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Abstract 

This  research  effort  examines  inertial  navigation  system  aiding  using  magnetic 
field  intensity  data  and  a  Kalman  filter  in  an  indoor  environment.  Many  current 
aiding  methods  do  not  work  well  in  an  indoor  environment,  like  aiding  using  the 
Global  Positioning  System.  The  method  presented  in  this  research  uses  magnetic 
field  intensity  data  from  a  three-axis  magnetometer  in  order  to  estimate  position  using 
a  maximum  -  likelihood  approach.  The  position  measurements  are  then  combined 
with  a  motion  model  using  a  Kalman  filter.  The  magnetic  field  navigation  algorithm 
is  tested  using  a  combination  of  simulated  and  real  measurements.  These  tests  are 
conducted  using  a  magnetic  field  intensity  map  of  the  entire  test  environment.  The 
result  of  these  tests  show  that  the  position  aiding  algorithm  is  capable  of  generating 
positon  estimates  from  real  data  within  less  than  1  meter  of  the  true  trajectory, 
with  most  estimates  .3  meters  away  from  the  true  trajectory  in  a  laboratory  hallway 
environment.  To  further  explore  the  capabilities  of  the  position  aiding  algorithm,  a 
leader- follower  scenario  is  implemented.  In  this  scenario,  the  follower  uses  magnetic 
field  intensity  data  collected  by  the  leader  to  estimate  its  current  position  and  attempt 
to  follow  the  leader’s  trajectory.  The  results  show  that  tracking  is  possible,  and  that 
the  measurement  span  of  the  leader  has  a  large  impact  on  the  result. 
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Magnetic  Field  Aided  Indoor  Navigation 


I.  Introduction 

Precision  navigation  has  become  an  important  aspect  of  military  and  commer¬ 
cial  applications.  The  advent  of  Global  Navigation  Satellite  Systems  (GNSS) 
has  provided  an  unprecedented  level  of  accuracy  to  a  wide  variety  of  users.  Once  these 
users  experience  a  specific  level  of  accuracy,  they  become  dependant  on  that  level  of 
accuracy  and  desire  more  accuracy  in  more  situations.  The  position  solutions  pro¬ 
vided  by  GNSS  have  provided  sub-meter  level  accuracy  in  many  applications,  but  the 
positioning  solutions  from  these  types  of  navigational  aids  are  only  available  when  the 
receiver  has  uninterrupted  access  to  at  least  four  satellites  [9].  While  overall  satellite 
coverage  of  the  Earth  has  increased,  environments  such  as  urban  canyons  and  inside 
buildings  can  prevent  the  acquisition  of  the  required  satellite  signals.  Without  ade¬ 
quate  satellite  coverage,  the  sub-meter  level  positioning  solutions  cannot  be  obtained. 
At  the  same  time,  there  is  an  increasing  desire  to  develop  autonomous,  miniature 
vehicles.  The  navigation  of  these  systems  is  generally  completed  with  inertial  navi¬ 
gation  systems  aided  by  GNSS  position  solutions.  If  the  GNSS  position  solutions  are 
not  available,  these  miniature  vehicles  will  not  be  able  to  navigate  with  the  required 
level  of  accuracy.  This  research  will  investigate  the  feasibility  of  using  magnetic  fields 
to  aid  an  inertial  system  when  GNSS  signals  are  not  available. 

The  research  presented  here  examines  the  uniqueness  of  magnetic  field  variations 
from  one  location  to  the  next  in  an  indoor  environment.  Using  this  uniqueness,  the 
feasibility  of  using  magnetic  held  variations  to  accurately  estimate  the  trajectory  of 
a  vehicle  is  presented.  Finally,  the  magnetic  aided  position  algorithm  developed  to 
estimate  a  vehicle’s  trajectory  is  applied  to  a  trajectory  tracking  implementation. 
The  following  research  examines  the  spatial  variations  of  magnetic  held  intensities, 
but  does  not  include  an  in-depth  analysis  of  the  variations  with  respect  to  time. 
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The  use  of  magnetic  field  variations  to  aid  outdoor  navigation  has  been  suggested 
by  various  authors  [3, 16]  and  successfully  implemented  by  Wilson,  Kline-Schoder, 
Kenton,  Sorensen,  and  Clavier  [19].  The  methods  proposed,  and  used,  implement  a 
terrain  navigation  algorithm,  where  the  terrain  map  is  replaced  by  a  map  of  magnetic 
field  intensities,  to  determine  the  position  based  on  the  measured  magnetic  field  in¬ 
tensity.  The  approach  presented  in  this  research  is  based  on  a  multiple  beam  terrain 
navigation  approach  originally  developed  for  submarine  navigation  [11],  However,  the 
algorithm  has  been  adapted  to  use  a  magnetic  field  intensity  map  instead  of  a  ter¬ 
rain  map  and  three-axis  magnetometers  instead  of  a  depth/terrain  height  measuring 
device. 

1.1  Modes  of  Operation 

The  magnetic  aided  position  algorithm  is  implemented  using  two  different  ap¬ 
proaches.  The  first  approach  estimates  a  vehicle’s  trajectory  using  a  magnetic  field 
intensity  map  of  the  entire  area  to  be  traversed,  which  provides  a  position  solution 
relative  to  the  area  that  is  included  in  the  map.  The  second  method  uses  magnetic 
field  intensity  data  collected  by  a  lead  vehicle,  or  the  leader.  The  leader  then  passes 
this  information  to  a  second  vehicle,  the  follower,  that  uses  the  magnetic  data,  com¬ 
bined  with  the  position  estimate  of  the  leader,  to  estimate  its  position  relative  to  the 
leader. 

1.2  Thesis  Overview 

Chapter  II  begins  with  a  brief  overview  of  the  components  used  in  an  inertial 
navigation  system  (INS)  and  the  types  of  errors  associated  with  these  devices.  A 
performance  analysis  of  three  different  grades  of  INS  is  shown  to  demonstrate  the  need 
for  position  aiding.  The  performance  analysis  is  followed  by  a  brief  description  of  the 
Earth’s  magnetic  fields  and  the  sources  of  variations  of  the  main  field.  The  history 
of  using  magnetic  fields  to  aid  in  navigation  is  then  covered,  to  include  animals  that 
find  there  way  around  the  Earth  using  magnetic  fields.  The  final  section  of  Chapter  II 
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introduces  and  explains  the  multiple  beam  terrain  navigation  approach  that  was  the 
starting  point  for  this  research. 

The  third  chapter  of  this  thesis  covers  the  methods  used  in  the  design  of  the  mag¬ 
netic  aided  position  algorithm.  The  chapter  begins  by  detailing  the  equipment  used 
and  required  for  implementation  of  this  aiding  algorithm.  The  next  section  in  Chap¬ 
ter  111  describes  the  magnetic  held  intensity  map  generation  process.  This  section 
includes  information  on  map  grid  point  spacing,  as  well  as  the  layout  and  orientation 
of  the  test  environment.  Once  the  map  generation  has  been  covered,  the  system  model 
used  in  this  research  is  defined  and  the  magnetic  aided  position  algorithm,  as  imple¬ 
mented,  is  explained.  The  final  piece  of  Chapter  111  is  the  leader-follower  algorithm. 
This  section  explains  the  different  method  used  for  map  generation,  as  well  as  the 
design  of  the  tracking  controller  used  on  the  follower  to  aid  in  tracking  the  estimated 
trajectory  of  the  leader. 

Chapter  IV  presents  the  results  generated  using  the  methods  presented  in  Chap¬ 
ter  III.  The  chapter  begins  by  showing  the  result  of  the  map  generation  process. 
Following  the  map  generation,  the  ensemble  statistics  from  100  Monte  Carlo  (MC) 
runs  are  presented  and  compared  with  the  filter  estimates  to  show  the  accuracy  of 
the  system  model  implemented  in  the  Kalman  filter.  In  addition  to  the  MC  results, 
the  position  errors  associated  with  using  real  measurements  are  shown  and  explained. 
The  final  set  of  results  show  the  performance  of  the  leader- follower  algorithm.  The 
leader- follower  algorithm  magnified  some  underlying  problems  with  the  magnetic  aid¬ 
ing  position  algorithm.  These  problems  are  explained  and  some  options  for  minimizing 
their  effects  are  described,  implemented,  and  the  results  are  presented. 

The  thesis  closes  with  a  summary  of  the  information  presented  in  the  preceding 
sections  and  chapters.  Chapter  V  closes  with  a  list  of  recommendations  that  will 
improve  the  results  and  make  the  magnetic  aided  position  algorithm  more  robust. 
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II.  Background 


NAVIGATION  on  the  Earth  requires  a  few  key  elements.  The  first  is  a  reference 
frame  and  the  ability  to  move  between  different  frames  using  coordinate  trans¬ 
formations.  The  coordinate  transformation  relies  on  the  angular  relationship  between 
reference  frames  to  rotate  one  frame  onto  another.  The  coordinate  transformations 
and  positioning  information  are  obtained  using  information  provided  by  gyroscopes 
and  accelerometers.  In  order  to  properly  understand  the  research  presented  in  this 
document,  the  concepts  of  coordinate  transformations  and  reference  frames  must  be 
understood.  Following  a  brief  description  of  coordinate  transformations  and  reference 
frames,  the  relationship  between  accelerometers,  gyroscopes,  and  coordinate  transfor¬ 
mation  is  defined. 

As  part  of  this  relationship  definition,  the  errors  common  to  accelerometers  and 
gyroscopes  are  presented.  Due  to  the  errors  inherent  in  these  inertial  sensors,  inertial 
navigation  systems,  must  be  aided.  While  numerous  aiding  techniques  are  available, 
the  Kalman  Elter  method  is  used  throughout  this  research  and  is  explained  in  this 
chapter.  The  Kalman  filter  aiding  method  can  be  successfully  used  to  aid  inertial 
systems  with  many  different  types  of  position  measuring  devices.  This  chapter  will 
conclude  with  a  look  at  some  emerging  techniques  that  use  the  Earth’s  magnetic  field 
to  aid  navigation  systems. 

2. 1  Coordinate  Transformations 

Navigation  on  the  earth  uses  numerous  reference  frames.  A  reference  frame  is 
defined  by  Titterton  and  Weston  as  “the  set  of  axes  to  which  the  measurements  and 
estimated  quantities  generated  within  an  inertial  system  are  referenced”  [16].  There 
are  many  resources  that  can  be  used  to  define  these  reference  frames  [17].  In  addition 
to  being  able  to  understand  how  each  reference  frame  relates  to  the  earth,  it  is  also 
important  to  understand  how  these  frames  relate  to  each  other.  While  “the  concept  of 
navigation  reference  frames  is  an  important  fundamental  for  expressing  the  position, 
velocity,  and  orientation  of  a  body”  [17],  coordinate  transformations  are  the  tools 
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used  to  make  this  information  mean  something.  Without  the  ability  to  move  from 
one  reference  frame  to  another,  vehicles  in  different  reference  frames  would  not  be 
able  to  determine  their  position  in  relation  to  each  other. 

The  Direction  Cosine  Matrix  (DCM)  is  one  way  of  rotating  between  reference 
frames.  The  DCM  uses  geometry  and  vector  math  to  relate  the  two  reference  frames. 
The  DCM  uses  “the  inner  product  of  each  unit  basis  vector  in  one  frame  with  each  unit 
basis  vector  in  another  frame”  [17].  The  standard  notation  for  the  DCM  is  Cha)  where 
a  is  the  reference  frame  that  is  being  converted  from  and  b  is  the  reference  frame  being 
converted  to.  The  angles  used  to  determine  the  DCM  are  provided  by  gyroscopes  in 
the  navigation  system.  A  vector  expressed  in  frame  a  ( xa )  can  be  rotated  into  frame 
b  ( xb )  using 

xb  =  Cbaxa  (2.1) 

2.2  Strapdown  Inertial  Navigation  Systems 

Strapdown  inertial  navigation  systems  (SINS)  are  used  to  determine  a  vehicle’s 
position  on  or  over  the  Earth  by  determining  the  vehicle’s  attitude  and  motion.  There 
are  many  different  ways  to  make  a  strapdown  navigation  system,  but  they  all  con¬ 
tain  the  same  major  components.  The  differences  between  SINS  are  driven  by  the 
application  of  that  particular  SINS.  The  main  factor  in  determining  what  a  partic¬ 
ular  SINS  will  look  like  is  the  accuracy  requirement  for  that  particular  application. 
The  accuracy  of  a  SINS  is  generally  expressed  as  a  rate  by  which  the  SINS  deviates 
from  the  actual  position  (i.e.  x  meters  per  sec/min/hour).  For  the  purposes  of  this 
research,  the  components  that  make  up  a  SINS  will  be  broken  down  into  two  main 
categories:  accelerometers  and  gyroscopes.  Each  of  these  components  are  necessary 
for  the  vehicle  to  calculate  its  attitude  and  position  with  respect  to  the  earth.  The 
overall  accuracy  of  the  SINS  comes  from  the  accuracy  of  these  two  components.  The 
sources  of  error  for  each  of  these  devices  will  be  discussed  in  their  respective  sections. 
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2.2.1  Accelerometers.  Accelerometers  measure  the  acceleration  of  an  ob¬ 
ject  [2],  The  vehicle’s  position  can  be  found  by  integrating  the  acceleration  twice. 
Fraden  states  that  this  becomes  an  important  truth  when  dealing  with  applications 
that  are  in  a  “noisy  environment”  (this  means  an  environment  that  has  some  sort  of 
condition  that  would  make  the  sensor  believe  it  is  accelerating  in  a  direction  that  the 
aircraft  is  not  actually  moving  in.  i.e.,  vibrations).  This  is  important  because  in  order 
to  calculate  velocity  and  acceleration  from  position,  it  is  necessary  to  take  the  first 
and  second  derivative  of  the  position,  respectively.  Taking  the  derivative  of  a  “noisy” 
signal  increases  the  level  of  noise  in  the  signal  to  the  point  of  making  it  unusable  or 
highly  inaccurate  [2],  Since  accelerometers  measure  acceleration,  there  is  no  need  to 
take  the  derivative,  just  the  integral,  which  handles  “noisy”  signals  well. 

Titterton  and  Weston  go  into  the  mechanics  and  the  theory  behind  accelerome¬ 
ters  in  depth.  For  this  research,  only  a  top-level  description  is  needed.  Accelerometers 
will  measure  acceleration  generated  from  the  force  exerted  by  the  aircraft  to  which  it 
is  mounted  as  well  as  the  force  of  gravity.  Therefore,  the  output  of  the  accelerome¬ 
ter  must  have  the  gravitational  acceleration  acting  on  it  subtracted  out  at  any  given 
time  [16]. 

As  stated  in  Section  2.2,  there  are  various  sources  of  error  within  accelerometers 
that  will  diminish  the  overall  performance  of  a  SINS.  A  full  description  of  these  are 
located  in  Titterton  and  Weston.  A  brief  description  is  given  in  Table  2.1. 


Table  2.1:  General  Accelerometer  and  Gyroscope  Errors  [16] 


Type  of  Error 

Error  Description 

Fixed  Bias 

Displacement  from  zero  “when  the  applied  acceleration 
is  zero” 

Scale-Factor  Errors 

Non-exact  “ratio  of  change  in  the  output  signal  to  a 
change  in  the  input  acceleration” 

Alignment  Errors 

Sensor  axes  is  non-orthogonal  due  to  “manufacturing 
imperfections” 
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Accelerometers  provide  the  linear  accelerations  needed  to  calculate  how  the 
vehicle  is  moving  in  the  inertial  frame  with  respect  to  the  body  frame.  Generally, 
there  is  one  accelerometer  oriented  in  each  of  the  three  Cartesian  axes,  (x,  y,  z).  This 
information  is  needed  in  the  navigation  frame  of  reference  to  be  of  use.  As  mentioned 
in  Section  2.1,  a  coordinate  transformation  will  be  used  to  get  this  data  into  the  correct 
reference  frame.  Gyroscopes  provide  the  angular  orientation  needed  to  generate  this 
coordinate  transformation. 

2.2.2  Gyroscopes.  While  accelerometers  use  Newton’s  Second  Law  of  Motion 
to  calculate  the  acceleration  of  the  aircraft  [16],  the  gyroscope  (called  “gyro”  for  short) 
measures  the  rotation  of  the  aircraft’s  body  [2], 

Mechanical  gyros  are  the  foundation  for  all  other  types  of  gyros.  For  a  listing 
of  the  different  types,  see  [2]  and  [16].  SINS  generally  use  micro-machined  electrome¬ 
chanical  systems  (MEMS)  gyros  or  optical  gyros  [16].  This  paper  will  only  cover 
the  basic  operation  of  a  mechanical  gyro  because  the  optical  and  MEMS  gyros  were 
developed  using  the  basic  principles  of  mechanical  gyros.  The  typical  two-degrees-of- 
freedom  mechanical  gyro  is  made  up  of  a  spinning  wheel  (called  the  rotor),  an  inner 
and  outer  gimbal,  and  angle  pick-off  sensors.  When  the  rotor  is  spinning,  “it  defines  a 
direction  in  space  that  remains  fixed  in  the  inertial  reference  frame”  [16].  By  having 
this  fixed  frame  of  reference,  rotation  can  be  detected.  Titterton  and  Weston  give 
a  very  in-depth  look  at  the  physics  behind  this  phenomenon.  To  paraphrase  their 
description,  the  gimbals  of  the  gyro  are  fixed  to  the  aircraft  and  move  with  the  body 
reference  frame,  with  the  rotor  suspended  along  the  spin  axis  inside  the  gimbals,  it  is 
able  to  maintain  its  direction  of  rotation  and  stays  true  to  the  inertial  reference  frame. 
The  “orientation  of  the  case  (gimbals)  of  the  instrument  with  respect  to  the  direction 
of  the  spin  axis”  [16]  can  be  measured  and  then  used  in  the  coordinate  transformation 
to  find  out  the  attitude  of  the  aircraft  in  the  navigation  frame.  A  typical  SINS  will 
have  three  gyros,  one  to  measure  the  roll  rate,  one  to  measure  the  pitch  rate,  and  one 
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to  measure  the  yaw  rate.  These  rates  are  then  used  to  determine  the  attitude  of  the 
aircraft. 

Just  like  with  accelerometers,  there  are  errors  associated  with  gyros  and  they 
can  be  viewed  in  Table  2.1. 

Once  the  angle  rates  have  been  measured,  this  information,  along  with  the  mea¬ 
sured  accelerations  along  each  axis,  will  be  passed  to  on-board  computers  to  calculate 
the  actual  position  of  the  aircraft.  The  accelerometers  and  gyroscopes  are  packaged 
together  in  an  inertial  measurement  unit  (IMU).  Once  the  on-board  computer  system 
is  added,  the  total  system  is  called  an  inertial  navigation  system  (INS). 

2.2.3  Inertial  Navigation  System  Aiding.  Inertial  navigation  systems  need  to 
be  aided  when  used  over  extended  periods  of  time.  The  definition  of  extended,  in  this 
case,  is  based  on  the  grade  of  INS  being  used  and  the  application.  Table  2.2  shows 
the  magnitudes  of  the  INS  errors  associated  with  three  grades  of  inertial  systems: 
Navigation  (H764-Q),  Tactical  (HG-1700),  and  Commercial  (Crista  IMU). 

Figure  2.1  shows  the  difference  in  uncertainty  as  a  function  of  time  for  each 
grade,  using  the  parameters  listed  in  Table  2.2.  From  this  figure,  it  is  clear  that  a 
Commercial  grade  INS  would  not  be  sufficient  if  the  application  required  a  solution 
within  1  deg  of  latitude  after  30  minutes  of  travel.  Instead,  the  navigation  grade  INS 
would  be  necessary  to  achieve  this  level  of  accuracy. 

Navigation  grade  inertial  systems  are  larger  in  size  and  more  expensive  than 
tactical  and  commercial  grade  inertial  systems.  For  systems  that  are  expendable, 
such  as  missiles,  bombs,  micro- air  vehicles  (MAVs),  etc.,  the  cheaper  the  system  the 
better.  However,  these  systems  require  a  high  level  of  accuracy  to  be  effective.  These 
realities  have  led  to  many  different  aiding  techniques  to  assist  the  inertial  systems 
with  their  navigation  solutions. 


(a)  Navigation  Grade  (b)  Tactical  Grade  (c)  Commercial  Grade 

Figure  2.1:  Inertial  navigation  system  performance  comparison,  (a)  The  latitude 

uncertainty  of  a  typical  navigation-grade  INS  over  a  30  minute  period. 

(b)  The  latitude  uncertainty  of  a  typical  tactical- grade  INS  over  a  30  minute  period. 

(c)  The  latitude  uncertainty  of  a  typical  commercial-grade  INS  over  a  30  minute 
period. 

2.3  Aiding  Techniques 

Kalman  filtering  has  proven  to  be  an  effective  algorithm  when  using  aiding 
measurements  with  an  INS  [16].  Many  navigation  measurement  systems  are  avail¬ 
able  to  aid  the  INS.  Some  of  these  techniques  use  sensors  on-board  the  vehicle,  such 
as  Doppler  radar,  altimeters,  magnetic  measurements,  terrain-referenced  navigation, 
scene  matching,  and  continuous  visual  navigation  [16].  Other  techniques  are  external 
to  the  vehicle,  such  as  radio  navigation  aids,  satellites,  star  trackers,  or  ground-based 
radar  trackers  [16].  For  the  purposes  of  this  research,  INS-aiding  via  magnetic  mea¬ 
surements  will  be  discussed. 

2.3.1  Kalman  Filtering.  A  Kalman  filter  (KF)  is  “an  optimal  recursive  data 
processing  algorithm”  [8]  that  is  used  to  generate  an  estimate  of  the  states  based 
on  current  measurement  values  and  all  previous  measurements.  However,  the  KF 
does  not  require  specific  knowledge  of  previous  measurements.  Instead,  the  KF  uses 
the  information  stored  in  the  previous  estimate  and  covariance  associated  with  this 
estimate  to  account  for  this  prior  information.  There  are  three  basic  assumptions  that 
must  be  met  in  order  for  the  KF  to  be  the  optimal  estimator. 
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Tabic  2.2:  IMU  Performance  Parameters  from  [18] 


Parameters  (units) 

Crista  IMU 

HG-1700 

H764-G 

Sampling  interval  (ms) 

5.0 

10.0 

3.906 

Gyro  bias  sigma  (deg  /hr) 

1800 

1.0 

0.0015 

Gyro  bias  time  constant  (hr) 

1 

1 

1 

Angular  random  walk  (deg  / \/Tvr) 

2.23 

0.3 

0.002 

Gyro  scalefactor  sigma  (PPM) 

10000 

150 

5 

Accel  bias  sigma  (m/s2) 

0.196 

0.0098 

2.45  x  10"4 

Accel  bias  time  constant  (hr) 

1 

1 

1 

Velocity  random  walk  (m/s/\/kr) 

0.261 

0.57 

0.0143 

Accel  scalefactor  sigma  (PPA4) 

10000 

300 

100 

These  assumptions  are  that  the  system  dynamics  can  be  modeled  as  a  linear 
system  and  that  measurement  noises  are  both  white  and  Gaussian  [8].  With  these 
assumptions  in  mind,  the  system  model  used  in  a  KF  is  comprised  of  a  dynamics 
matrix,  F,  a  matrix  that  captures  the  intensities  of  the  noises  driving  the  system 
model,  Q,  and  a  matrix  that  describes  how  control  inputs  affect  the  outcome  of  the 
system,  B.  These  matrices  satisfy  the  stochastic  differential  equation 


x(f)  =  F(f)x(f)  +  B(f)u(f)  +  G(f)w(f)  (2.2) 

where  w(f)  represents  the  white,  Gaussian,  driving  noises  on  the  system  [8].  The  ma¬ 
trix  Q  is  defined  as  E  {w(t)w(t)J  }  =  Q S(t).  In  real-life  applications,  measurements 
are  generally  available  at  discrete  points  in  time.  Therefore,  in  order  to  combine  the 
state  estimate  with  the  measurements,  Equation  2.2  must  be  discretized,  which  re¬ 
sults  in  a  stochastic  difference  equation.  The  equivalent  stochastic  difference  equation 
for  Equation  2.2  which  is  developed  in  [8],  becomes, 

x(4+ 1)  =  $(4+i,  4)x(4)  +  Bd(4)u(4)  +  wd(tk)  (2.3) 
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where 


At  —  tk+i  —  tk 

*(tk+i,tk)  =eF^At 

Bd=  [tk+1  <f>(tk+1,T)B(r)dT  (2’4) 

Jtk 
rtk+ 1 

E[wd(tk)wdT(tk)}  =  /  $(tfc+i,  r)G(r)Q(r)Gr(r)$r(tfc+i,  r)dr. 

Jtk 

2. 3. 1.1  Propagation  and  Measurement  Updates.  Using  the  system 
model  as  defined  above,  the  first  step  in  the  Kalman  filtering  process  is  to  propagate 
the  current  information  forward  in  time.  This  step  is  a  prediction  of  how  the  states 
will  change  over  a  specific  time  interval,  At.  For  example,  if  a  vehicle  moves  forward 
from  an  initial  position,  xo,  at  a  constant  velocity  of  60  mph  for  10  minutes  (At),  the 
vehicle  will  be  exactly  10  miles  ahead  of  its  initial  position,  if  the  system  model  is 
perfect  and  there  are  no  other  error  sources  associated  with  the  vehicle.  The  system 
dynamics  matrix,  F,  contains  the  behavior  information  of  the  system,  in  the  case 
of  the  example,  it  would  reflect  a  constant  velocity.  The  actual  velocity  of  60  mph 
would  be  provided  in  the  initial  condition  for  the  system.  The  KF  advances  the  state 
estimates  based  on  the  system  model,  but  then  estimates  the  uncertainty  of  that  state 
estimate  by  adding  the  white,  Gaussian  noises.  This  is  done  by  applying  x(t^)  =  x0 
and  P  (t£)  =  Po  to 

x(tfc+1)  =  $(4+ 1,  tfc)x(t£)  +  Bdu(tk)  (2.5) 

P(tfc+1)  =  &(tk+i,tk)P(f£)&T(tk+i,tk)  +  Qd  (tk)-  (2.6) 

where  x(t^)  is  the  post-measurement  update  state  estimate  at  time  tk,  x(Fj)  is 
the  propagated  (pre-measurement)  state  estimate  at  time  tk+ 1,  P (t£)  is  the  post 
measurement  filter  covariance,  and  P(tk+1)  is  the  propagated  filter  covariance. 
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With  the  state  estimates  propagated,  the  next  step  is  to  combine  the  measure¬ 
ment  data,  when  it  is  available,  with  the  propagated  state  estimate.  This  is  done 
using 


z(tfc)  =  H(4)x(4)  +  v(tfc)  (2.7) 

K  (tk)  =  P(^)HT(4)[H(4)P(t-)HT(tfc)  +  R  (t*)]"1  (2.8) 

*(#)  =  x(*ife)  +  K(4)[z(4)  -  H(tfc)x(t*)]  (2.9) 

P(4+)  =  P(^)  -  K(4)H(4)P(47)  (2.10) 


where  z (£*.)  is  the  measurement  equation,  H(tfc)  is  the  output  matrix,  and  v(tfc)  is  the 
measurement  corruption  noise,  which  is  white  and  Gaussian  [8].  The  matrix  K(f*,) 
is  the  Kalman  Gain.  This  is  a  ratio  that  tells  the  filter  how  much  weight  should  be 
applied  to  the  filter  estimate  and  to  the  measurement.  For  instance,  if  the  system 
estimate  has  a  large  uncertainty  in  relation  to  the  measured  state  values,  then  K(f&) 
would  drive  the  filter’s  post-measurement  estimate,  x(^),  towards  a  value  closer  to 
the  filter’s  estimate  prior  to  measurement  incorporation,  x(f)T). 

2. 3. 1.2  Kalman  Filter  Implementation  For  Nonlinear  Systems.  As 
mentioned  earlier,  the  above  KF  equations  are  based  on  the  assumption  that  the 
system  dynamics  are  linear.  However,  systems  operating  in  the  real-world  are  seldom 
linear  over  all  operating  conditions.  In  order  to  meet  this  assumption,  the  system 
dynamics  must  be  linearized.  There  are  two  different  KF  types  that  use  linearization, 
the  linearized  KF  (LKF)  and  the  Extended  KF  (EKF)  [7].  Each  type  uses  the  same 
procedure  to  linearize  the  dynamics  equations,  but  differ  in  the  operating  point  used  to 
linearize  about.  The  linearization  procedure  begins  with  a  nonlinear  system,  described 
by  Maybeck  as 

x  =  f[x(f),u  (2.11) 

The  linearized  KF  is  developed  by  calculating  the  Taylor  series  that  describes  this 
differential  equation  about  some  nominal  condition  and  then  ignoring  the  higher- 
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order  terms  [8].  For  the  general  case  presented  here,  the  Taylor  series  expansion  is 
found  to  be 

Sx(t)  =  F  (t)Sx(t)  +  B(f)<5u(f)  (2.12) 

where 

dh  ...  dh 

dx\  dxn 

dfn  .  .  .  dfn 

dxi  dxn 

dh  ...  dfi_ 

du\  dun 

dfn.  .  .  .  dfn 

dui  dun 

and  xQ(t),  u 0(t),t  are  the  nominal  conditions.  The  complete  derivation  of  these 

equations  can  be  viewed  in  [8]. 

Using  this  method,  there  are  two  options  for  choosing  the  nominal  conditions. 
The  first  method  is  to  design  the  KF  to  use  a  dynamics  matrix  that  does  not  change 
after  the  initial  linearization,  meaning  f(t)  is  linearized  about  a  known  nominal  tra¬ 
jectory  or  about  a  set  of  conditions  known  to  be  marginally  nonlinear  [7] .  This  is  the 
LKF.  The  second  method  relinearizes  the  dynamics  matrix  about  each  filter  estimate. 
This  method,  used  in  the  EKF,  takes  into  account  the  fact  that  the  filter  is  providing 
the  optimal  estimate  at  any  given  time,  which  makes  this  method  more  accurate  in 
cases  where  a  nominal  trajectory  is  not  known  in  advance  or  when  the  system  varies  in 
a  highly  nonlinear  fashion  [7].  An  example  of  the  differences  in  performance  between 
these  two  methods  can  be  viewed  in  [7]. 

2.3.2  Magnetic  Field  Based  Navigation.  Magnetic  fields  have  been  observed 
by  humans  for  centuries.  Plato  wrote  of  rocks  that  were  magnetically  attracted  to 
other  rocks  in  400  BC  [1].  The  Chinese  developed  a  magnetic  compass  between  300 
and  200  BC  that  was  used  to  align  construction  with  the  Earth’s  magnetic  fields;  this 
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compass  was  made  to  look  like  the  big  dipper,  so  that  the  end  of  the  bowl  would 
point  in  the  horizontal  northward  direction,  also  like  the  big  dipper  [1],  From  these 
early  observations  and  uses,  merchants  began  using  compasses  to  navigate  to  their 
various  trading  locations.  This  was  the  start  of  the  compass  navigation  used  to  today. 
Compasses  use  the  magnetic  properties  of  repulsion  and  attraction  to  determine  which 
direction  is  north  relative  to  the  users  current  position.  As  demonstrated  by  Columbus 
and  many  other  early  explorers,  not  understanding  the  nature  of  the  Earth’s  magnetic 
field  can  cause  confusion  when  trying  to  use  the  Earth’s  magnetic  held  to  navigate 
on  the  Earth  [1], 

2. 3. 2.1  Brief  Description  of  the  Earth’s  Magnetic  Field.  Magnetic 
north,  or  the  point  that  compasses  point  to,  is  currently  offset  from  the  geographic 
north  pole,  or  the  Earth’s  spin  access,  by  about  12°  in  latitude  [1,14].  However,  the 
pole  location  is  not  stationary,  which  causes  the  location  of  magnetic  north  to  rotate 
around  the  geographic  pole  every  2,000  to  3,000  years  [1].  This  offset  is  caused  by  the 
nature  of  the  Earth’s  magnetic  held. 

The  Earth’s  magnetic  held  is  often  viewed  as  a  large  dipole  magnet,  that  is 
a  magnet  that  has  two  opposing  poles  (generally  termed  north  and  south  poles)  at 
each  end  [1],  Campbell  points  out  that  the  Earth’s  magnetic  held  acts  like  a  dipole, 
but  is  not  actually  a  dipole  magnet.  The  magnetic  held  that  surrounds  the  Earth 
comes  from  currents  that  are  induced  by  “the  outer-core  region  of  the  Earth” ,  which 
is  composed  of  “a  hot  and  dense  liquid  of  highly  conducting  nickel-iron” ,  and  “the 
Earth’s  spin  and  shape”  [1].  Together  these  characteristics  form  a  current- loop  that 
generates  a  magnetic  held  that  acts  similar  to  a  dipole  magnet.  Figure  2.2  shows  the 
different  layers  of  the  Earth,  as  well  as  the  magnetic  held  surrounding  it  [1].  From 
this  illustration,  it  can  be  seen  that  the  Earth’s  magnetic  held  can  be  observed  from 
any  position  on  the  Earth.  While  the  main  held  of  the  Earth’s  magnetic  held  is  fairly 
constant,  there  are  a  number  of  factors  that  can  cause  variation  in  the  intensity  of 
the  magnetic  held  at  any  given  time  and  place. 
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Figure  2.2:  Illustration  of  the  Earth’s  main  dipole  magnetic  field.  The  liquid  outer 
core,  along  with  the  Earth’s  rotation  is  believed  to  be  the  source  of  the  Earth’s  dipole 
like  magnetic  field  [1]. 

Campbell  highlights  two  of  the  naturally  occurring  variations.  The  first  is  caused 
by  an  alteration  in  the  Earth’s  electrical  conductivity,  which  can  be  caused  by  “a  major 
change  in  the  groundwater  content  at  a  deep  subsurface  fracture. .  .or  when  a  highly 
conductive  active  magma  chamber  at  a  volcanic  site  moves  before  an  eruption”  [1], 
The  second  cause  is  a  result  of  a  change  in  the  magnetic  domain  boundaries  of  rocks 
due  to  increased  external  stress  [1].  Campbell  describes  this  by  stating  this  change  is 
brought  about  “as  a  result  of  the  loading  of  rock  surfaces  as  a  major  dam  is  filled  or  at 
a  volcano  as  a  result  of  a  change  in  the  magma  chamber  pressure  on  the  surrounding 
rock  material”  [1]. 

In  addition  to  variations  of  this  nature,  deposits  in  the  Earth’s  crust  also  con¬ 
tribute  to  local  variations  in  the  observed  magnetic  field  [10,19].  These  local  variations 
have  more  of  an  effect  closer  to  the  Earth’s  surface.  As  altitude  is  increased,  the  inten¬ 
sity  of  the  magnetic  field  has  less  variation  due  to  these  deposits  in  the  crust  [10,19]. 
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Some  other  factors  that  can  add  to  these  variations  are  the  effects  on  the  Earth’s 
magnetic  held  due  to  magnetic  storms,  the  moon,  the  sun,  and  the  ionosphere  [10]. 

There  are  also  artificial  disturbances  that  perturb  the  Earth’s  magnetic  held  [10]. 
These  artihcial  disturbances  are  caused  by  electrical  currents  running  through  any 
type  of  metal  or  conducting  structure.  In  buildings,  walls  are  often  reinforced  with 
steel  rebar,  newer  construction  projects  use  steel  studs  in  interior  walls.  Steel  beams 
used  to  support  the  hoors  of  buildings  add  to  the  problem,  as  well  as  pipes,  wires, 
and  electric  motors  or  equipment  [10,13]. 

2. 3. 2. 2  Biological  Examples  of  Magnetic  Field- Aided  Navigation.  The 
Earth’s  magnetic  held,  as  described  in  Section  2.3.2. 1  is  used  by  a  number  of  animals 
to  help  them  find  their  way  to  mating  areas,  migration  locations,  and  moving  around 
their  habitats  [6].  Two  animals  that  are  known  to  use  the  Earth’s  magnetic  held  to 
gain  positional  information  during  their  long-distance  migrations  are  the  loggerhead 
sea  turtle  and  the  pied  flycatcher  [6]. 

The  loggerhead  sea  turtles  migrate  from  their  hatching  location  along  the  east 
coast  of  Florida  to  an  area  called  the  North  Atlantic  gyre  and  then  back  to  the  south¬ 
eastern  United  States  [6].  Experiments  conducted  by  Lohmann  and  Lohmann  showed 
that  loggerhead  turtles  can  measure  the  magnetic  inclination  angle  of  the  Earth’s 
magnetic  held  and  the  magnetic  held  intensity  [6],  and  that  they  use  this  to  help 
them  navigate.  The  central  European  pied  flycatcher’s  begin  their  migration  in  cen¬ 
tral  Europe  and  then  hy  a  course  that  prevents  them  from  having  to  cross  areas  that 
are  not  conducive  to  survival  (i.e.  the  Alps,  the  Mediterranean  Sea,  and  the  central 
Sahara)  [6].  The  path  taken  by  these  birds  has  them  hy  in  a  southeasterly  direction 
and  then,  at  a  specihc  point,  turn  and  continue  their  migration.  The  experiment  used 
to  investigate  this  phenomenon  used  the  magnetic  helds  associated  with  the  various 
locations  along  the  path  of  their  migration  [6].  The  result  was  the  pied  flycatchers 
turned  to  the  same  direction,  and  the  direction  of  their  normal  migration  route,  when 
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the  magnetic  field  was  artificially  changed  to  match  that  of  the  true  magnetic  field  at 
the  location  where  a  heading  change  would  normally  occur  [6]. 

These  are  just  two  examples  of  animals  naturally  using  the  characteristics  of  the 
Earth’s  magnetic  field  to  navigate  their  way  around  the  Earth.  Several  other  animals 
have  undergone  the  same  sort  of  experiments  and  the  use  of  magnetic  fields  has  been 
shown  [6].  These  studies  have  helped  humans  to  better  understand  how  animals 
navigate  around  the  globe  and  given  researchers  ideas  for  new  methods  of  navigation 
using  the  Earth’s  magnetic  field  as  a  source  for  position  and  attitude  information. 

2. 3. 2. 3  Navigation  Techniques  Using  the  Earth’s  Magnetic  Fields. 
Early  on  in  global  explorations,  explorers  believed  that  their  compasses  pointed  to  a 
“magnetic  mountain”  [1],  as  opposed  to  a  magnetic  field  generated  by  the  composition 
of  the  Earth.  As  Spanish  merchants  began  to  move  outside  of  “a  narrow  longitudi¬ 
nal  sector  along  the  west  coast  of  Africa”,  it  became  obvious  that  this  “magnetic 
mountain”  was  not  a  true  stationary  mountain  aligned  with  true  north  as  previously 
thought  [1].  This  observation  led  scientists  to  begin  examining  the  true  nature  of 
the  observed  magnetic  field.  Based  on  their  studies,  new  methods  of  navigation  were 
developed.  One  of  these  breakthroughs  was  the  use  of  declination  angles  to  account 
for  the  observed  difference  between  magnetic  north  and  true  north.  These  are  angles 
that  can  be  added  (or  subtracted)  to  the  magnetic  north  heading  to  find  the  heading 
for  true  north.  For  maritime  applications,  as  the  ship  experiences  changes  in  longi¬ 
tude,  the  declination  angle  will  also  change  [1].  To  keep  track  of  this,  the  declination 
angles  must  be  known  for  all  locations  along  the  path  of  travel.  This  advance  allowed 
for  explorers,  merchants,  and  anyone  else  wanting  to  navigate  using  a  compass  and 
map  to  accurately  find  their  destination.  This  type  of  navigation  is  still  taught  today 
and  is  effective  when  GPS  is  not  available  and  in  environments  where  a  compass  will 
provide  accurate  readings. 

Magnetic  fields  are  also  used  to  determine  which  direction  a  vehicle  is  facing,  also 
known  as  its  heading.  This  is  an  important  tool  as  vehicles  become  smaller  because 
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heading  measurements  generally  come  from  gyroscopes,  and  accurate  gyroscopes  are 
generally  bulky  [16].  For  outdoor  applications,  this  type  of  aiding  is  very  effective, 
assuming  the  declination  angle  is  known.  For  indoor  applications,  the  use  of  electronic 
compasses  is  not  as  straight  forward.  As  mentioned  in  Section  2. 3. 2.1,  there  are  many 
disturbances  present  in  the  indoor  environment.  While  there  are  a  number  of  different 
ways  to  reduce  the  impacts  of  these  disturbances,  the  heading  provided  in  indoor 
applications  still  does  not  meet  the  accuracy  needs  of  many  indoor  applications  [13]. 

It  is  apparent  that  there  are  many  different  ways  to  use  the  Earth’s  magnetic 
field  in  navigation.  However,  each  of  the  above  methods  are  impacted  by  variations 
in  the  Earth’s  magnetic  field.  Titterton  and  Weston  suggest  using  a  map  of  magnetic 
anomalies  to  estimate  the  vehicle’s  position  [16].  This  method  relies  on  magnetic 
anomaly  maps  and  the  stability  of  such  anomalies.  However,  since  the  anomalies  are 
the  measurement,  the  anomalies  will  not  interfere  with  the  measurements  like  when 
using  an  electronic  compass  indoors  for  heading  reference  [13].  Instead,  assuming 
they  are  stable,  or  vary  in  a  quantifiable  way,  the  more  variations  in  a  given  location, 
the  more  unique  the  magnetic  “fingerprint”  [3].  Figure  2.3  shows  an  example  of  this 
“fingerprint”  for  an  indoor  environment. 

This  technique  has  been  demonstrated  in  the  outdoor  environment  on  an  air¬ 
craft  [19].  Wilson,  Kline-Schoder,  Kenton,  Sorenson,  and  Clavier  use  maps  of  “aero- 
magnetic  anomalies”,  which  are  found  by  taking  the  difference  of  the  expected  mag¬ 
netic  magnitude  and  the  actual  measured  local  field  [19].  The  approach  outlined 
in  [19]  uses  only  the  magnitudes  of  the  magnetic  field  anomaly  vectors.  By  doing  this, 
Wilson  et  al.  do  not  take  advantage  of  all  the  information  available  from  the  magnetic 
field.  The  experiment  is  conducted  using  a  three-axis  magnetometer,  but  the  three 
measured  values  are  then  combined  to  find  the  total  magnitude  of  the  magnetic  field. 
The  terrain  navigation  approach  outlined  in  the  next  section  will  show  a  way  to  use 
these  three  different  values  together,  in  order  to  come  up  with  a  position  solution. 
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Figure  2.3:  Map  of  a  one  square  meter  magnetic  fingerprint  generated  from  mag¬ 
netic  field  data  collected  in  a  hallway  at  the  Air  Force  Institute  of  Technology. 

2. 3. 2-4  Magnetic  Field  Navigation  Using  a  Terrain  Navigation  Approach. 

Terrain  navigation,  in  general,  is  based  on  measuring  the  terrain  topography  and 
then  correlating  this  measurement  with  a  position  on  a  map  [11].  The  depth  or 
height  measurements  often  used  in  these  methods  are  similar  to  the  magnitudes  of  the 
magnetic  field.  Early  methods  of  terrain  navigation  used  single  beam  measurements 
to  determine  the  location;  this  method  was  not  very  accurate  in  flat-bottomed  areas, 
which  do  not  have  enough  variation  to  determine  the  location  of  the  measured  position 
[11].  Modern  terrain  navigation  methods  use  many  more  beams  to  measure  a  broader 
area,  which  provides  more  information;  hence  making  it  easier  to  pinpoint  the  vehicle’s 
position  [11].  The  terrain  navigation  approach  described  by  Nygren  in  [11]  describes 
a  method  for  combining  multiple  depth  measurements  with  some  sort  of  navigation 
system  (i.e.  INS,  Doppler  velocity  log  (DVL),  or  a  system  that  measures  how  far 
the  submarine  has  traveled  by  counting  propeller  turns)  to  determine  a  submarine’s 
position. 

Nygren’s  method  begins  by  defining  the  system  model  as 

t  =  0, 1,  2,  •  •  •  (2.14) 

(2.15) 


xm  =  xt  +  ut  +  vt; 
y  t  =  h(xt)  +  et 
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where  u*  is  defined  as  the  displacement  from  the  previous  position,  x4,  at  time  t,  vt 
is  the  random  error  associated  with  the  navigation  system  that  provided  x(,  h(xt) 
is  the  function  that  maps  a  specific  value  of  x(  to  the  measurement  domain,  and  yt 
is  the  actual  measurement  collected  by  the  sensor,  which  has  a  measurement  error, 
et  [11].  Equation  2.14  is  defined  as  the  propagation  equation  and  Equation  2.15  is  the 
measurement  equation  [11],  These  equations  can  be  combined  using  methods  similar 
to  those  discussed  in  Section  2.3.1. 

If  et  is  Gaussian,  the  likelihood  function  is 

L(xq  yt)  =  1  =exp  ( Y]  (: Vt,k  ~  hk(xt)) ]  (2.16) 

J(i^  V  J 


where  a \  is  the  measurement  error  variance  and  N  is  the  number  of  measurements 
to  be  incorporated  [11].  Equation  2.16  is  a  mathematical  way  to  determine  how  well 
a  measurement  correlates  to  the  information  contained  in  the  map  [11].  Therefore, 
the  maximum  value  of  L(xt;  yt)  is  located  at  the  point  most  likely  to  be  the  position 
of  the  vehicle.  Nygren  points  out  that  if  this  information  is  used  by  itself,  then  the 
position  estimate  is  considered  a  maximum  likelihood  estimate  (MLE).  For  the  case 
of  the  submarine,  or  any  vehicle  with  an  additional  navigation  system,  the  MLE  can 
be  combined  with  the  position  estimate  from  the  propagation  equation  using  Bayes’ 
rule  [11].  As  defined  in  [5],  Bayes’  rule  is 


P[B\A] 


P[A<1B] 


(2.17) 


which  can  be  written  equivalently  as 

where  P[A|P]  is  the  probability  of  A  given  all  values  of  B. 


(2.18) 
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For  the  application  in  [11],  P[B\A]  =  p  (xt+i|Ym),  P[A\B]  =  L  (yt+i|xt+i), 
P[B]  =  p(xt+i|Yf),  and  P  [A]  becomes  a  normalizing  constant,  C,  to  ensure  the 
result  integrates  to  one.  The  result  of  this  is  that  the  posterior  probability  density 
function  (pdf),  meaning  the  pdf  associated  with  the  position  estimate  after  the  po¬ 
sition  measurement  is  incorporated,  is  found  by  combining  the  propagated  position 
pdf  with  the  result  of  Equation  2.16.  Mathematically,  this  can  be  written  as 

Mx„1|Y,+,)~L(y,"|x‘^(x‘+i|Y,).  (2.19) 

Once  the  posterior  pdf  is  found,  the  position  measurement  can  be  calculated 
using  the  point  of  maximum  likelihood,  found  using  Equation  2.16,  as  the  position 
measurement  and  the  uncertainty  associated  with  this  position  measurement,  R ,  is 
the  radius  of  curvature  of  the  posterior  pdf  at  the  point  of  maximum  likelihood  [11]. 
Then,  as  outlined  by  Nygren  and  using  the  KF  techniques  discussed  in  Section  2.3.1, 
the  position  estimate  is  updated  using  this  measurement  data. 

2.4  Summary 

This  chapter  has  shown  why  position  aiding  of  inertial  systems  is  necessary  for 
navigation,  ft  has  also  described  the  nature  of  the  magnetic  fields  that  are  present 
outdoors  and  indoors,  as  well  as  the  nature  of  the  disturbances  that  create  variations 
in  these  fields.  Numerous  applications  of  using  magnetic  fields  for  navigation  were 
presented,  followed  by  an  introduction  to  the  approach  used  in  this  research.  Chap¬ 
ter  111  will  describe  the  methodology  used  to  develop  the  magnetic  aided  position 
algorithm  used  in  this  research. 
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III.  Methodology 


This  chapter  will  step  through  the  design  process  that  was  used  for  creating 
a  magnetic  held  aided  navigation  system.  The  design  process  begins  with 
having  the  necessary  hardware  and  software  to  complete  the  data  collection  and  the 
navigation  algorithm  because  the  first  piece  of  required  data  for  the  design  process  is 
the  magnetic  held  intensity  map.  Without  this  initial  data  collection,  feasibility  of  the 
approach  could  not  be  verified.  Therefore,  this  chapter  will  describe  how  the  area  to 
be  mapped  was  setup,  as  well  as  how  the  magnetic  held  intensity  map  was  constructed 
from  the  magnetic  held  intensity  information.  Once  the  map  description  is  complete, 
the  system  model  is  defined  and  used  to  generate  the  propagation  equations  that  are 
required  for  implementation  of  the  KF.  Following  the  development  of  the  propagation 
equations,  the  algorithm  for  generating  the  position  measurement  is  detailed.  The 
KF  update  equations  are  then  outlined. 

With  the  basic  aiding  algorithm  described,  the  next  portion  of  this  chapter 
covers  the  implementation  of  a  leader- follower  algorithm.  This  algorithm  uses  a  lead 
vehicle  to  measure  the  magnetic  held  as  it  passes  through  an  area.  The  lead  vehicle 
then  passes  the  magnetic  held  data,  its  estimated  trajectory,  and  any  turn  commands 
to  the  follower  vehicle.  The  follower  vehicle  then  traverses  the  same  area  using  only 
the  provided  data  and  a  motion  measurement  device.  The  designed  magnetic  held 
aiding  algorithm  is  used  in  conjunction  with  the  data  from  the  lead  vehicle. 

3. 1  Required  Equipment 

One  of  the  hrst  steps  in  collecting  and  processing  data  is  to  be  certain  that 
equipment  is  available  and  that  it  will  meet  the  requirements  of  the  design.  For  this 
research,  the  only  required  equipment  is  three  three-axis  magnetometers  and  a  laptop 
that  can  collect  and  process  all  data  pertaining  to  the  magnetic  held  intensity  map 
and  the  magnetic  aided  position  algorithm. 
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3.1.1  Three- Axis  Magnetometers.  Initially,  the  three-axis  magnetometers 
present  in  the  MIDG  II®  were  analyzed  for  their  use  as  the  magnetometers  in  this  re¬ 
search.  However,  the  specifications  for  the  MIDG  II®  magnetometer  were  not  available 
from  the  manufacturer.  While  the  magnetometer  data  is  easily  accessible  through  the 
device  interface,  it  is  only  intended  to  aid  the  heading  information  in  the  INS  by  pro¬ 
viding  the  direction  of  the  magnetic  field  intensity  vector,  but  not  the  magnitude  [15]. 
Therefore,  it  is  not  meant  to  be  a  stand-alone  magnetometer  and  very  susceptible 
to  temperature  fluctuations  and  the  readings  might  not  be  stable  over  long  periods 
of  operation.  Due  to  the  design  requirements  of  the  device,  the  magnetic  readings 
produced  by  the  MIDG  II®  cannot  be  converted  to  a  standard  unit  of  measure.  In 
addition  to  not  accurately  measuring  the  magnitude  of  the  magnetic  held  vector,  the 
readings  from  each  MIDG  II®  magnetometer  were  corrupted  by  scale  factor  errors 
and  biases.  These  errors  could  be  accounted  for  using  the  least-squares  approach,  but 
when  the  sensors  were  rotated  90°,  the  readings  on  the  sensor  did  not  reflect  such 
a  rotation  (i.e.,  if  the  x-axis  reading  was  900  counts,  when  rotated  90°  the  y-axis 
should  read  900  counts  (using  a  standard  right-handed  Cartesian  coordinate  frame)). 
Without  being  able  to  accurately  account  for  this  error  a  different  sensor  was  required. 

The  three-axis  magnetometer  used  for  this  research  was  the  Honeywell  HMR2300®. 
The  HMR2300®  is  a  stand-alone  three-axis  magnetometer  designed  to  provide  mag¬ 
netic  field  intensity  information  for  a  number  of  applications.  Figure  3.1  shows  the 
HMR2300®  as  used  in  this  research.  The  HMR2300®  has  a  measurement  range  of  ±2 
gauss  with  a  resolution  of  <  70  /igauss  [4],  For  reference  purposes,  the  Earth’s  mag¬ 
netic  field  ranges  from  approximately  .3  gauss  at  the  equator  to  .6  gauss  at  the 
poles  [14],  Based  on  the  magnitude  of  the  Earth’s  magnetic  field  intensity,  the  se¬ 
lected  magnetometers  should  be  able  to  measure  all  variations  about  the  Earth’s  main 
held. 


3.1.2  Laptop.  The  laptop  used  in  this  research  was  a  Dell®  Latitude  D630 
with  an  Intel®  Centrino®  2.0  GHz  processor.  Overall,  it  is  a  standard  notebook 
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Figure  3.1:  Photo  of  the  Honeywell  HMR  2300  as  packaged  for  research 

personal  computer.  LabVIEW®  was  used  to  collect  the  magnetic  field  intensity  data 
and  Matlab®  was  used  to  complete  all  data  processing  in  a  post-processing  mode. 

3.2  Magnetic  Field  Intensity  Map 

As  mentioned  in  Section  2. 3. 2. 4,  a  map  of  the  magnetic  field  intensities  must 
exist  in  order  to  use  the  terrain  navigation  approach.  The  map  must  contain  loca¬ 
tion  specific  magnetic  held  intensity  data.  This  section  will  describe  the  two  main 
steps  taken  to  generate  these  maps  of  magnetic  held  data.  The  hrst  step  covers  the 
data  collection  methods,  while  the  second  outlines  the  process  used  to  transform  the 
measured  data  into  a  useable  format. 

3.2.1  Magnetic  Map  Data  Collection.  Following  the  approach  used  in  [11], 
the  map  data  is  actually  a  grid  of  measurements  at  specihc  locations.  The  envi¬ 
ronment  chosen  to  demonstrate  this  technique  was  two  connected  hallways  near  the 
Advanced  Navigation  and  Technology  Center  at  the  United  States  Air  Force  Institute 
of  Technology  (AFIT).  Figure  3.2  is  a  blueprint  of  the  area  used  for  this  experiment, 
with  the  actual  hallways  used  outlined  in  red. 

A  right-handed,  Cartesian  reference  frame  is  used  in  the  development  of  the 
algorithm,  with  the  Z-axis  being  positive  in  the  downward  direction.  Figure  3.2 
shows  the  navigation  reference  frame  in  relation  to  the  hallway  and  Figure  3.3  shows 
the  navigation  reference  frame  with  respect  to  the  body  reference  frame.  The  positive 
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Figure  3.2:  Layout  of  test  environment  used  for  testing  magnetic-aided  INS.  The 
area  is  a  hallway  at  the  Air  Force  Institute  of  Technology. 

X-axis  is  considered  forward  from  the  starting  location  (which  is  denoted  by  a  star 
in  Figure  3.2),  while  the  X-axis  extends  from  left  to  right.  Using  this  orientation,  the 
grid  spacing  along  the  A"-axis  of  the  hallway  needs  to  have  the  same  spacing  as  the 
sensors  to  ensure  the  grid  points  line  up  with  the  grid  points  generated  for  the  side 
hallway.  To  achieve  the  symmetric  grid  needed  to  build  the  map,  the  magnetometers 
are  arranged  in  a  row,  with  .381  meters  (15  inches)  of  separation  between  each  sensor. 
Therefore,  the  spacing  between  grid  points,  in  both  the  X-direction  and  X -direction, 
is  .381  meters.  To  ensure  position  accuracy,  a  tape  measure  was  used  to  place  pieces 
of  masking  tape  every  .381  meters  in  the  X-direction,  and  then  every  .381  meters  in 
the  X-direction  down  the  side  hallway.  The  uppercase  X,  X,  Z  denote  the  navigation 
frame,  whereas,  described  in  the  following  section,  lowercase  x ,  y,  z  denote  the  body’s 
reference  frame  (the  magnetometers  are  aligned  with  the  body  frame).  As  shown  in 
Figure  3.3,  when  the  angle  0  is  0°,  the  two  reference  frames  are  aligned. 
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Figure  3.3:  Relationship  between  the  navigation  reference  frame  and  the  body 

reference  frame. 

The  actual  grid  data  was  collected  using  the  cart  and  sensor  array  shown  in 
Figure  3.4.  A  makeshift  plumb-bob  was  used  to  ensure  that  the  sensor  array  was 
aligned  with  the  grid  point.  Once  aligned,  a  reading  was  taken  and  stored  in  a  text 
hie.  Due  to  the  width  of  the  hallway,  the  sensor  array  only  covers  half  of  the  hallway, 
ffence,  this  procedure  is  completed  twice  for  the  main  hallway  and  twice  for  the  side 
hallway,  resulting  in  a  measurement  being  taken  at  310  different  locations,  which 
amounts  to  930  grid  points  of  magnetic  held  intensity  data.  At  each  point,  a  full 
3-axis  magnetometer  reading  was  taken.  With  the  data  collected  in  a  data  array,  the 
next  step  is  to  turn  this  information  into  useful  map  data. 

3.2.2  Magnetic  Map  Data  Processing.  Each  three- axis  magnetometer  sends 
its  magnetic  held  intensity  data  as  a  package.  Table  3.1  shows  a  subset  of  the  data 
collected  using  the  magnetometers  with  labels  to  illustrate  the  data  structure.  By 
loading  this  information  into  Mat  lab®  ,  the  data  can  be  reorganized  to  generate  three 
different  magnetic  held  intensity  maps,  one  for  each  axis. 
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Figure  3.4:  Photo  of  the  data  collection  vehicle.  Sensor  array  is  located  on  board 
in  front  of  the  laptop. 

As  shown  in  Figure  3.2,  the  area  to  be  mapped  is  L-shaped.  In  order  to  convert 
the  magnetic  data  into  a  rectangular  grid  for  Matlab®  purposes,  the  measurements 
from  the  side  hallway  must  be  transposed  and  then  aligned  with  the  main  hallway 
data.  The  area  below  the  side  hallway,  but  next  to  the  main  hallway,  is  unmapped 
and  zeros  are  assigned  to  this  region.  Table  3.2  illustrates  the  resulting  data  structure. 

When  using  grid  points  as  a  reference  of  position,  the  smaller  the  grid  spacing 
the  better.  However,  measuring  a  large  number  of  grid  points  is  time  consuming  and 
sometimes  prohibited  by  measurement  devices.  In  order  to  achieve  a  smaller  space 
between  measured  grid  points,  the  Matlab®  interpQ  function  is  used  to  resample  the 
collected  data  at  a  closer  interval.  By  using  the  interpi)  function,  the  grid  spacing 
is  reduced  from  .381  m  to  .0381  m  in  all  directions.  Interpolating  this  data  is  valid 
because  only  a  few  points  are  needed  between  the  existing  points  and,  from  Table  3.1, 
the  variation  between  points  appears  to  be  fairly  smooth. 

3.3  System  Model 

As  mentioned  in  Section  3.2.1,  the  reference  frame  used  is  not  the  body  frame 
of  the  vehicle.  Instead,  the  relationship  between  the  navigation  reference  frame  and 
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Table  3.1:  Example  of  measured  magnetic  fields.  This  data  is  loaded  into  Mat  lab® 
and  manipulated  to  form  three  magnetic  field  intensity  maps,  one  for  each  axis.  The 
units  of  the  measured  data  is  counts  (1  gcmss=15,000  counts ).  Each  data  point  is 
separated  by  .381  meters  (15  inches). 


Left  Sensor 

Center  Sensor 

Right  Sensor 

x-Axis 

y-Axis 

z-Axis 

x-Axis 

y-Axis 

z-Axis 

x-Axis 

y-Axis 

z-Axis 

372 

1442 

3802 

462 

824 

3067 

1594 

1671 

4008 

394 

1447 

3833 

427 

824 

3081 

1267 

1744 

3931 

445 

1514 

3858 

458 

897 

3103 

1344 

1968 

3999 

525 

1587 

3859 

553 

983 

3123 

1519 

2017 

4073 

481 

1020 

2816 

476 

1045 

3103 

1423 

2005 

4069 

394 

1677 

3844 

451 

1103 

3062 

1352 

2233 

3955 

405 

1667 

3820 

526 

1111 

2997 

1638 

2196 

3920 

383 

1629 

3804 

530 

1022 

2955 

1884 

1520 

3849 

390 
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3891 

Table  3.2:  Data  structure  following  reorganization  to  place  data  in  positions  that 
correlate  to  their  true  positions  in  the  test  environment. 
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the  body  frame  is 


VY 
Vx 

where  Vy  and  Vx  are  the  velocities  in  the  direction  of  the  Y  and  X  axes  in  the 
navigation  frame,  Vj,  is  the  velocity  of  the  vehicle  in  the  body  frame  and  -0  is  the 
heading  angle  of  the  body  with  respect  to  the  navigation  frame.  The  relationship 
between  the  body  frame  and  the  navigation  frame  can  be  seen  in  Figure  3.3.  For  this 
research,  the  heading  (if;)  is  assumed  known  at  all  times. 
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Therefore,  the  system  model  of  the  experimental  vehicle  is  defined,  in  the  navi¬ 
gation  reference  frame,  as 
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where  Py  and  Px  are  the  positions  in  the  X  and  Y  directions,  uyY  and  Uyx  are  the 
control  inputs  used  to  control  the  velocity,  and  wVY  and  wVx  are  the  white,  Gaussian 


noise  driving  the  system,  characterized  by  E[w(t)wr(t  +  r) 


Q 


3-4  Magnetic  Aided  Position  Algorithm 

The  magnetic  aided  position  algorithm  is  composed  of  three  parts.  The  algo¬ 
rithm  begins  with  the  system  propagation  equations,  followed  by  the  position  mea¬ 
surement  generation  algorithm,  and  then  the  measurement  update  equations.  The 
propagation  and  update  equations  are  the  standard  KF  equations  presented  in  Sec¬ 
tion  2.3.1,  while  the  measurement  generation  algorithm  uses  the  ideas  presented  in  [11] 
and  Section  2. 3. 2. 4. 
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3-4-1  Propagation  Equations.  The  propagation  equations  are  found  by  ap¬ 
plying  the  principles  explained  in  Section  2.3.1,  which  begin  with  taking  Equation  3.2 
from  the  continuous-time  domain  to  the  discrete-time  domain  using  a  time  step  (At) 
of  .1  sec  and  using  aa  =  .08  m/s2.  The  resulting  propagation  equations  are 
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Once  the  previous  state  estimate  and  covariance  have  been  propagated  forward 
in  time,  the  next  step  is  to  use  the  propagated  state  estimate  to  generate  a  position 
measurement  based  on  the  magnetic  field  intensity  measurement. 

3-4-2  Measurement  Generation.  In  terrain  navigation,  certain  features  or 
characteristics  must  be  matched  to  determine  the  position  and  then  that  matched 
position  is  used  as  the  position  measurement  for  the  KF.  The  same  holds  true  when 
using  magnetic  field  intensity  data  [3,19].  The  magnetometers  measure  the  magnetic 
field  intensity  and  then  the  measurement  generation  algorithm  determines  how  this 
measurement  relates  to  the  vehicle’s  position  via  a  mapping  function  [11], 

Using  the  process  outlined  in  Section  2. 3. 2. 4,  the  first  step  in  finding  the  position 
associated  with  a  particular  measurement  is  to  combine  the  propagated  pdf  with  the 
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result  of  the  likelihood  function  to  determine  the  location  that  best  matches  the 
magnetic  reading.  This  is  accomplished  by: 

1.  Generating  a  discrete  pdf  that  accurately  describes  the  propagated  position 
estimate. 

2.  Calculating  the  likelihood  function  for  all  possible  locations. 

3.  Combining  the  results  from  steps  1  and  2  using  Equation  2.19  to  determine  the 
position  measurement,  which  is  located  at  the  maximum  of  the  resulting  pdf. 

4.  Calculating  the  measurement  noise  intensity,  R. 


3-4-2. 1  Propagated  PDF  Generation.  The  propagated  pdf  contains 
all  of  the  information  about  the  propagated  state  estimate  generated  by  the  filter. 
For  the  two-dimensional  Gaussian  application,  all  necessary  statistical  information  is 
contained  in  a  bivariate  Gaussian  pdf,  represented  by  its  mean  and  standard  deviation. 
These  are  computed  as  the  propagated  KF  state  estimate  (x(tAr)),  and  covariance 
P(tfc).  Since  the  magentic  field  intensity  data  used  in  the  likelihood  function  is 
discrete,  the  pdf  must  also  be  discrete  to  apply  Bayes’  rule.  For  a  bivariate  Gaussian 
pdf,  the  probability  associated  with  a  specific  position  on  the  pdf  can  be  determined 
using  this  equation,  modified  from  [8], 
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where  crY  and  crx  are  the  square  roots  of  the  associated  terms  from  P(t^),  xY{t) 7)  and 
xx(tk)  are  the  KF  propagated  filter  estimates  (mean  values),  rYX  is  the  correlation 
coefficient  between  the  two  random  variables,  and  £y  and  are  the  coordinates 
of  the  points  where  the  probability  is  unknown.  The  discrete  posterior  pdf  is  then 
calculated  by  comparing  all  points  (this  is  done  by  varying  £y  and  £X  in  Equation  3.5) 
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in  a  desired  area  to  the  filter  estimate.  In  Matlab®  ,  this  can  be  done  in  a  single  step 
using  an  array  of  different  and  £x- 

3. f.2. 2  Likelihood  Function.  In  Equation  2.16,  N  is  the  number  of  dif¬ 
ferent  measurements  to  be  used  in  determining  the  likelihood  of  a  particular  location. 
Since  three-axis  magnetometers  provide  three  different  magnetic  field  intensities,  one 
along  each  axis,  IV  =  3  for  this  application.  Therefore,  Equation  2.16  becomes 

L(xq  yt)  =  1  -.exp  ( V  (ytr k  -  hk(xt)) ]  .  (3.6) 

^/((2vr)3ae2)  V  ae  k= i  / 

By  using  the  Matlab®  capability  to  do  math  element-by-element  in  one  com¬ 
mand,  the  a;- axis  measurement,  the  y- axis  measurement,  and  the  z-axis  measurement 
can  be  compared  with  their  respective  maps  in  one  command.  The  result  of  this 
Matlab®  command  is  an  array  of  numbers  that  show  how  likely  it  is  that  a  given  lo¬ 
cation  is  the  location  of  the  vehicle,  given  the  magnetic  field  intensity  measurement. 
The  next  step  in  the  process  is  to  combine  the  propagated  pdf  with  the  result  from 
the  likelihood  function. 

3. f.2. 3  Application  of  Bayes’  Rule.  As  noted  previously,  the  final  step 
in  determining  the  position  measurement  is  completed  by  using  Equation  2.19.  The 
result  from  Section  3.4.2. 1  and  3. 4. 2. 2  make  up  the  numerator  of  the  right-hand  side 
of  Equation  2.19,  repeated  here  for  clarity.  The  maximum  value  of  this  data  set,  is  the 
point  that  will  be  used  as  the  position  measurement,  z (tk),  in  the  update  equations. 

3. f.2. 4  Measurement  Noise  Intensity.  From  Section  2.3.1,  it  is  known 
that  the  KF  needs  a  value  for  the  measurement  noise  intensity.  For  applications  such 
as  GPS,  the  measurement  noise  of  the  GPS  measurement  is  the  value  assigned  to  R. 
In  this  case,  that  will  not  be  sufficient  because  the  measurements  generated  by  the 
magnetometers  are  transformed  into  position  measurements,  but  the  errors  are  not 
directly  transformed  from  gauss  to  a  unit  of  length.  In  addition  to  the  errors  not  being 
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in  the  proper  units,  any  information  that  could  be  gained  from  the  post-Bayes’  rule 
pdf  is  dependant  upon  the  filter’s  uncertainty  and,  for  a  KF,  the  process  noise  and 
the  measurement  noise  (i.e.,  Q (f^)  and  R (tf.),  respectively)  need  to  be  uncorrelated  or 
accounted  for  by  an  augmented  state  matrix.  The  method  proposed  by  Nygren  in  [11] 
uses  the  radius  of  curvature  of  the  posterior  pdf  to  calculate  R,  but  then  augments  the 
KF  to  account  for  this  correlated  noise.  Instead  of  using  the  correlated  noise  method, 
a  new  method  was  developed  to  remove  the  correlation. 

The  new  process  begins  with  the  point  of  maximum  likelihood.  Using  this  point 
as  the  center,  an  array  of  indices  is  created  in  the  X-direction  and  in  the  Y -direction. 
The  measured  magnetic  held  intensity  is  then  compared  with  the  magnetic  held  in¬ 
tensities  of  the  surrounding  locations  using  Equation  3.6.  The  result  of  the  likelihood 
function  is  then  normalized  to  make  the  result  a  pdf.  The  standard  deviations  of 
the  two  resulting  pdfs  (one  for  the  X-direction  and  one  for  the  Y -direction)  will  be 
the  square  root  of  the  measurement  noise  intensity,  or  the  standard  deviation  of  the 
dataset.  For  a  Gaussian  pdf,  ~68%  of  the  data  is  included  within  a  la-band.  To 
calculate  the  standard  deviation  of  experimental  pdfs,  the  point  on  the  left  side  of 
the  mean  where  ~16%  of  the  data  is  in  the  tail  can  be  found  and  the  same  for  the 
right  tail.  The  distance  that  these  two  points  are  from  the  mean  of  the  pdf  are  then 
averaged  to  calculate  the  standard  deviation  ( cry  and  ax)  of  the  measurement  noise 

(Ty  0 

intensity,  resulting  in  R  = 

_  0 

3-4-3  Measurement  Update  Equations.  Once  the  position  measurement  used 
for  updating  the  KF  estimate  is  calculated,  the  update  portion  of  the  KF  remains 
the  same  as  described  in  Section  2.3. 1.1.  The  only  difference  is  that  Equation  2.7  is 
replaced  by  the  position  associated  with  the  maximum  value  of  the  likelihood  and 
propagated  pdf  combination.  Essentially,  the  measurement  generation  algorithm  de¬ 
scribed  in  Section  3.4.2  replaces  the  mapping  function  and  measurement  noise  that 
are  represented  in  Equation  2.7. 
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3.5  Leader-Follower  Methodology 

With  the  process  for  aiding  a  navigation  system  with  magnetic  field  data  devel¬ 
oped,  the  next  step  is  to  investigate  the  feasibility  of  using  the  magnetic  aided  position 
algorithm  in  a  leader-follower  scenario.  The  leader-follower  scenario  is  defined  by  hav¬ 
ing  the  first  vehicle,  the  leader,  move  through  an  area  while  measuring  the  magnetic 
field  intensity  along  its  path.  The  second  vehicle  would  come  along  some  time  later 
with  the  magnetic  field  intensity  data  from  the  leader,  the  trajectory  estimate  from 
the  leader,  and  any  turn  commands  that  were  executed  by  the  leader.  The  leader  is 
envisioned  as  being  a  vehicle  that  has  several  different  sensors  on-board  to  aid  in  its 
travel  through  the  indoor  environment,  as  well  as  three  magnetometers.  The  second 
vehicle  is  envisioned  as  being  a  less  equipped  vehicle,  with  only  one  magnetometer 
and  some  sort  of  motion  measuring  device  (e.g.,  INS,  odometer,  etc.).  The  method 
described  in  Section  3.4  is  used  as  the  basis  of  this  leader-follower  approach. 

3.5.1  Map  Generation.  In  Section  3.2,  the  map  generated  was  composed 
of  six  different  sensor  measurements  and  covered  a  rectangular,  equally  spaced  grid. 
For  this  application,  the  points  of  the  map  are  generated  according  to  where  the 
leader  estimates  it  has  traveled  (which  is  different  from  where  it  actually  traveled). 
In  order  to  accomplish  this  non-uniform  grid,  the  Matlab®  function  griddata ()  can 
be  used.  The  leader  collects  magnetic  field  intensity  data  at  a  regular  interval.  In 
this  case,  that  interval  is  every  .5  sec.  Each  magnetic  held  intensity  measurement  is 
recorded  by  the  leader,  along  with  the  estimate  of  the  leader’s  position  at  the  time  of 
measurement.  The  magnetic  held  intensity  measurements  and  the  leader’s  position 
estimate  are  sent  to  the  follower  vehicle.  The  follower  vehicle  compares  its  three-axis 
magnetometer  measurements  with  those  provided  by  the  leader  to  determine  where 
it  is  located  in  reference  to  the  leader’s  estimated  trajectory. 

3.5.2  Follower  Vehicle  Control.  The  follower  vehicle  system  model  is  similar 
to  the  system  model  presented  in  Section  3.3  except  there  is  an  additional  control 
input,  u2(f).  This  control  input  is  used  to  track  the  estimated  trajectory  of  the  lead 
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Diagram  3.1:  Graphical  example  of  the  error  (A  p).  The  error  A  p  is  used  to  determine 
the  amount  of  control  applied  to  the  follower  to  track  the  reference  trajectory  x.r 

vehicle.  Therefore,  the  stochastic  differential  equation  used  to  describe  the  follower  is 

x(f)  =  F(t)x(t)  +  B(f)  (ui(t)  +  u 2(t))  +  G(t)w(t).  (3.7) 

where  the  second  controller,  ii2(t),  is  a  proportional  integral  derivative  (PID)  con¬ 
troller. 

The  PID  controller  was  chosen  because,  as  pointed  out  by  Ogata  in  [12],  PID 
controllers  are  applicable  to  most  control  systems  and,  after  attempting  a  proportional 
controller,  it  became  obvious  that  a  different  controller  was  needed.  The  PID  con¬ 
troller  combines  a  proportional  controller  with  an  integral  controller  and  a  derivative 
controller,  which  allows  more  flexibility  in  reaching  various  operating  conditions.  For 
a  tracking  controller,  the  setpoint  error  is  found  by  calculating  the  error  between  the 
actual  output  of  the  plant  and  the  reference  trajectory.  For  this  application,  the  goal 
is  to  ensure  the  follower  navigates  within  the  area  mapped  by  the  leader.  Therefore, 
the  cross-track  error  is  the  error  that  needs  to  be  minimized.  In  order  to  calculate 
the  cross-track  error  (AP),  the  distance  between  x(t£)  and  the  leader’s  estimated 
trajectory  must  be  found.  The  problem  with  calculating  this  error  is  that  it  cannot 
be  directly  calculated  because  the  leader’s  trajectory  is  not  defined  for  all  points  on 
the  map.  The  geometry  of  the  problem  is  shown  in  Diagram  3.1. 
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Gaussan  White  Noise 


Figure  3.5:  PID  controller  block  diagram  derived  from  [12]  built  in  Simulink  by 
Mat lab® 

Using  Diagram  3.1  as  a  reference,  the  distance  A p  is  calculated  by  first  finding 
the  distance  between  x(t^)  and  xrl,  which  is  the  closest  defined  point  on  the  reference 
trajectory  located  below  x(ijj').  With  the  vector  a  calculated,  the  next  step  is  to  find 
the  distance  between  xr]  and  the  point  c.  This  is  completed  by  calculating  a proj, 
which  is  the  vector  equivalent  to  a  projected  onto  vector  b.  By  adding  a proj  and  xrl, 
the  location  of  c  is  found.  Once  the  value  of  c  is  found,  A p  is  found  by  subtracting 
x(t£)  from  c. 

With  the  input  to  the  controller  defined,  the  final  step  in  designing  the  PID 
controller  is  to  determine  the  gain  parameters  that  will  ensure  system  stability  and 
provide  the  desired  performance.  The  PID  controller  uses  three  tunable  gain  param¬ 
eters  (Zip,  K, ,  and  Kd)  to  achieve  the  desired  system  performance.  By  tuning  these 
parameters,  the  response  time  of  the  system,  the  maximum  overshoot,  and  the  settling 
time  can  all  be  changed  to  meet  system  performance  requirements. 

Figure  3.5  is  the  block  diagram  of  the  total  system  used  in  the  follower  vehicle. 
The  dashed  box  labeled  w2  is  the  PID  controller.  As  mentioned  previously,  the  input 
to  the  PID  controller  is  the  cross-track  error. 
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Figure  3.6:  Step  response  of  PID  controller  used  to  control  the  path  of  the  follower, 
without  process  noise  applied. 


In  this  case,  the  controller  gain  values  were  found  by  using  the  Matlab®  Simulink 
toolbox.  The  block  diagram  pictured  in  Figure  3.5  was  executed  in  Simulink  with  vary¬ 
ing  values  for  the  three  gain  parameters  until  the  desired  performance  was  achieved, 
which  was  a  rise  time  less  than  1  sec,  with  minimal  overshoot  (this  was  defined  as  less 
than  .1  meters),  and  a  settling  time  as  close  to  2  sec  as  possible.  These  values  were 
achieved  with  Kp  =  10,  7Q  =  .05,  and  Kd  =  3.5.  The  step  response  of  this  controller, 


with  no  process  noise  and  U\ 


0 

0 


is  shown  in  Figure  3.6. 


Figure  3.7  shows  that  the  system  will  remain  stable  and  track  the  desired  trajec¬ 
tory  when  process  noise  is  present.  The  noise  that  was  added  for  this  step  response, 
was  white,  Gaussian  noise  with  the  same  intensity,  Q,  as  that  used  in  the  system 
model  introduced  in  Section  3.4.1.  From  these  figures,  it  can  be  seen  that  the  con¬ 
troller  is  stable  and  adequately  meets  the  desired  performance  characteristics. 


3. 6  Summary 

This  chapter  has  described  the  procedures  used  to  design  the  magnetic  aided 
position  algorithm.  Chapter  IV  will  show  the  results  generated  by  implementing  the 
processes  and  procedures  explained  in  this  chapter. 
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Figure  3.7:  Step  response  of  PID  controller  used  to  control  the  path  of  the  follower, 
with  process  noise  applied. 
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IV.  Implementation 


This  chapter  will  show  that  the  magnetic  held  variations  are  location  specific,  as 
well  as  discuss  the  performance  of  the  magnetic  aided  position  algorithm  in 
environments  that  have  small  variations  in  the  magnetic  held.  This  was  done  using  a 
simulation  that  uses  both  simulated  measurements  and  real  measurements.  The  simu¬ 
lated  measurements  were  used  to  show  the  magnetic  aided  position  algorithm’s  ability 
to  track  random  trajectories,  while  the  real  measurements  were  used  to  demonstrate 
that  magnetic  variations  are  stable  enough  in  time  to  be  used  in  such  an  application. 
Following  the  analysis  of  the  magnetic  aided  position  algorithm,  using  the  simula¬ 
tion,  the  results  from  the  leader- follower  implementation  will  be  examined.  It  will  be 
shown  that  the  leader- follower  implementation  is  more  susceptible  to  areas  of  small 
variation,  and  some  methods  for  minimizing  these  impacts  are  presented. 

4-1  Magnetic  Field  Intensity  Variations  By  Location 

To  verify  that  the  magnetic  held  intensity  is  location-specific,  three  different 
datasets  of  magnetic  held  intensity  were  collected  from  different  hallways  around  the 
AFIT  campus.  The  readings  from  the  magnetic  sensors  could  be  different  based 
on  actual  magnetic  held  intensity  in  that  area,  as  well  as  if  the  sensors  were  not 
moved  through  the  test  areas  in  the  same  orientation.  An  example  of  this  (using 
a  standard  right-handed  Cartesian  coordinate  system)  would  be  a  magnetometer’s 
x-axis  is  aligned  straight  ahead  and  the  reading  along  that  axis  is  .6  milli-gauss,  the 
reading  along  the  y-axis  is  .45  milli-gauss.  When  the  sensor  is  rotated  90  degrees  to  the 
right,  the  y-axis  is  now  facing  the  direction  the  x-axis  was  facing  initially.  The  reading 
along  the  y-axis  would  now  read  .6  milli-gauss,  while  the  x-axis  measurement  would 
be  -.45  milli-gauss.  To  show  that  the  variations  are  clue  to  the  environment  and  not 
the  orientation  of  the  sensors,  three  different  test  environments  were  chosen.  The  first 
hallway  chosen  runs  east  to  west  and  is  on  the  second  floor  of  an  academic  building. 
For  this  data  collection  effort,  a  single  magnetometer  was  moved  15.24  meters  (50  ft), 
from  east  to  west,  through  the  hallway.  Figure  4.1  shows  the  relationship  between  the 
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Figure  4.1:  The  orientations  of  the  hallways  used  for  demonstrating  the  uniqueness 
of  magnetic  field  intensities  in  an  indoor  environment.  Two  are  on  the  second  floor 
and  are  parallel  with  each  other,  while  the  third  is  located  on  the  third  floor,  directly 
above  hallway  number  1. 

three  hallways.  The  second  hallway  tested  was  chosen  because  it  is  located  directly 
above  the  first  hallway  tested.  Since  the  second  hallway  is  oriented  and  in  the  same 
position  as  the  first  (except  for  the  difference  in  height  because  one  is  on  the  second 
floor  and  the  other  is  on  the  third),  the  data  from  these  two  hallways  would  be  similar 
if  the  magnetic  field  intensities  are  not  unique  between  locations.  The  third  hallway 
was  chosen  to  demonstrate  the  differences  between  two  hallways  that  are  on  the  same 
floor  and  aligned  with  each  other.  As  can  be  seen  in  Figure  4.2,  each  of  the  hallways 
have  their  own  unique  magnetic  fingerprints  along  each  of  the  three  magnetometer 
axes. 


4-2  Magnetic  Field  Intensity  Map 

Based  on  the  method  described  in  Section  3.2,  the  magnetic  field  intensity  maps 
have  two  main  sections  of  data.  The  first  is  the  magnetic  field  intensity  data  collected 
in  the  main  section  of  the  hallway,  and  the  second  data  used  is  the  magnetic  field 
intensity  data  from  the  side  hallway.  The  result  of  combining  these  two  datasets 
can  be  viewed  in  Figure  4.3,  which  is  a  magnetic  field  intensity  map  generated  from 
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Figure  4.2:  The  magnetic  field  intensities  in  three  different  hallways,  all  oriented 

from  north  to  south,  were  compared  to  show  that  each  hallway  has  a  unique  mag¬ 
netic  field  intensity  fingerprint.  The  measurements  were  taken  using  one  three-axis 
magnetometer. 

the  x-axis  magnetometer  measurements.  The  data  extending  along  the  X-axis  is 
the  data  collected  in  the  main  hallway,  while  the  data  extending  along  the  X-axis 
is  the  side  hallway  data,  with  the  exception  of  the  zeros  that  are  entered  into  the 
map  as  described  in  Section  3.2.2.  While  Figure  4.3  is  a  good  illustration  of  how  the 
data  collected  by  the  magnetometers  is  organized,  it  does  not  provide  the  required 
level  of  visual  resolution  to  adequately  analyze  the  information  contained  in  the  map. 
Therefore,  the  magnitude  and  variation  of  the  data  in  the  two  hallways  can  be  seen 
better  when  viewed  separately. 

4-2.1  Main  Hallway  Magnetic  Field  Intensity  Map.  Figure  4.4  shows  the 
main  hallway’s  magnetic  field  intensity  map,  using  magnetic  field  intensity  data  col¬ 
lected  on  20  November  2008.  Since  the  magnetic  aided  positioning  algorithm  relies 
on  differences  in  the  magnetic  field  intensity  to  relate  a  magnetometer  reading  to 
a  position,  unique  patterns  and  values  of  intensity  are  desired.  For  the  HMR2300, 
the  measurement  noise  has  a  standard  deviation  of  approximately  25  counts,  which 
equates  to  approximately  1.67  milli-gauss.  Variations  that  are  significantly  larger  than 


41 


0  5  10  15 

Y-Direction  (meters) 

Figure  4.3:  Example  two-dimensional  magnetic  map  using  the  x-axis  magnetometer 
data. 

1.67  milli-gauss  will  be  largely  unaffected  by  measurement  noise.  Due  to  the  scale  of 
Figure  4.4,  it  is  difficult  to  ascertain  how  many  variations  are  below  this  threshold. 
Figure  4.5  shows  a  smaller  area  of  the  hallway  to  increase  the  resolution.  Based  on 
Figure  4.5,  the  main  hallway  does  have  variations  large  enough  to  be  seen  over  the 
magnetometer’s  noise. 


4-2.2  Side  Hallway  Magnetic  Field  Intensity  Map.  Figure  4.6  was  gener¬ 
ated  using  the  same  procedures  as  the  map  for  the  main  hallway.  Overall,  there  are 
numerous  variations  present,  but  to  see  if  the  variations  are  sufficiently  large,  the 
same  procedure  applied  to  Figure  4.4  was  applied  to  Figure  4.6.  Figure  4.7  is  the 
result  of  mapping  a  smaller  section  of  the  side  hallway.  The  result  is  the  same  as 
that  shown  with  the  main  hallway.  There  should  be  enough  variation  to  determine 
distinct  position  information  using  the  magnetic  aided  position  algorithm.  With  the 
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Figure  4.4:  The  main  hallway  magnetic  intensity  maps,  generated  per  procedures 
outlined  in  Section  3.2.  Each  plot  represents  a  different  magnetometer  axis. 
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Figure  4.5:  A  closer  look  at  the  variations  present  in  the  main  hallway.  Variations 
smaller  than  1.67  milli-gauss  will  not  be  visible  above  the  noise  of  the  magnetometer 
and  could  prevent  the  magnetic  aided  position  algorithm  from  estimating  an  accurate 
position  estimate. 

map  completed,  the  next  step  was  to  investigate  the  ability  of  the  magnetic  aided 
position  algorithm  defined  in  Chapter  III  to  estimate  a  vehicle’s  trajectory. 


4-3  Magnetic  Aided  Position  Algorithm  Results 

This  algorithm  is  implemented  using  two  different  approaches.  The  first  ap¬ 
proach  uses  a  map  of  an  entire  area,  which  was  measured  ahead  of  time,  to  estimate 
the  location  of  a  vehicle  within  the  mapped  area.  The  second  method  uses  magnetic 
field  intensity  data  collected  along  a  vehicle’s  path,  as  it  moves  through  an  area. 
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Figure  4.6:  The  side  hallway  magnetic  intensity  maps,  generated  per  procedures 

outlined  in  Section  3.2.  Each  plot  represents  a  different  magnetometer  axis. 


Figure  4.7:  A  closer  look  at  the  variations  present  in  the  side  hallway.  Variations 
smaller  than  1.67  milli-gauss  will  not  be  visible  above  the  noise  of  the  magnetometer 
and  could  prevent  the  magnetic  aided  position  algorithm  from  estimating  an  accurate 
position  estimate. 

The  second  method  is  used  for  a  relative  positioning  solution  (i.e. ,  where  is  the  sec¬ 
ond  vehicle  located  with  respect  to  the  first),  as  opposed  to  an  absolute  positioning 
solution  (where  is  the  vehicle  located  in  a  defined  area).  The  differences  in  these 
two  methods  only  impact  which  method  of  map  generation  to  use,  as  described  in 
Section  3.2.2  and  3.5.1.  Once  the  map  is  generated,  the  magnetic  aided  position  al¬ 
gorithm  uses  the  map  in  the  same  way  to  estimate  a  vehicle’s  position  relative  to  the 
positioning  system  associated  with  the  map. 
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4-3.1  Example  Discrete  Bivariate  Gaussian  PDF.  As  mentioned  in  Chap¬ 
ter  III,  the  first  step  in  the  magnetic  aided  position  algorithm  is  calculating  the  dis¬ 
crete  bivariate  Gaussian  pdf.  As  mentioned  previously,  this  pdf  is  centered  around  the 
propagated  state  estimate  and  uses  the  associated  propagated  filter  uncertainties.  For 


the  example  pictured  in  Figure  4.8,  the  position  state  estimate  is  x(tfc ) 


1.1565 

2.2438 


and  P (tk) 


0.0513  0 

0  0.4837 


The  area  in  view  was  reduced  to  show  more  detail. 


4-3.2  Example  Likelihood  Function  Result.  Using  the  same  location  as  in 
the  pdf  case,  Figure  4.9  shows  the  result  of  Equation  3.6.  The  measurements  for  this 
example  were  simulated  based  off  of  the  truth  data  used  in  generating  the  map.  For 
this  example,  it  is  clear  that  there  are  three  peaks.  Any  one  of  these  peaks  could  be  the 
actual  location  of  the  vehicle.  If  the  maximum  value  is  chosen  at  this  point  this  process 
becomes  a  maximum  likelihood  estimator  [11].  However,  this  result  is  combined  with 
the  bivariate  Gaussian  pdf  using  Bayes’  rule,  as  discussed  in  Section  3. 4. 2. 3. 


4-3.3  Example  Bayes’  Ride  Implementation  Example.  The  result  of  com¬ 
bining  the  likelihood  function  and  the  bivariate  Gaussian  pdf  is  shown  in  Figure  4.10. 
Since  the  two  peaks  of  the  likelihood  function  were  so  closely  positioned,  both  are 
still  present  after  combination  with  the  pdf.  However,  the  magnitudes  of  these  peaks 
have  been  scaled  according  to  their  location  from  the  filter’s  estimated  position.  The 
difference  between  the  height  of  the  peak  located  at  approximately  (1.75,1)  and  the 
one  located  at  approximately  (2.4,1)  is  now  noticeably  larger.  Also  notice  that  the 
peak  located  at  approximately  (1.5, 3. 2)  is  completely  removed  from  the  figure. 

Choosing  the  point  of  maximum  likelihood  from  the  combination  of  the  propa¬ 
gated  pdf  and  the  likelihood  result  could  result  in  the  wrong  location  being  chosen. 
Choosing  the  wrong  location  could  prevent  the  magnetic  aided  position  algorithm 
from  accurately  estimating  its  position.  For  the  basic  trajectory  estimation  case  to  be 
presented,  this  phenomenon  did  not  create  a  problem.  However,  in  the  leader- follower 
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Figure  4.8:  Example  discrete  propagated  probability  density  function  that  will  be 
combined  with  the  result  of  the  likelihood  function  using  Bayes’  rule. 
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Figure  4.9:  Example  discrete  likelihood  function  result  that  will  be  combined  with 
the  result  of  the  discrete  probability  density  function  using  Bayes’  rule 
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Figure  4.10:  Example  discrete  post-measurement  probability  density  function  gen¬ 
erated  using  Bayes’  rule  to  combine  Fig.  4.8  with  Fig.  4.9. 

implementation  it  did  create  a  problem.  In  order  to  reduce  the  effects  of  bad  measure¬ 
ments,  several  options  could  be  implemented  following  the  combination  of  the  pdf  and 
the  likelihood  function.  One  method  would  be  residual  monitoring.  If  a  measurement 
was  so  many  meters  away  from  the  estimate  (this  value  would  be  determined  based  on 
your  system  model)  that  measurement  would  be  ignored.  Another  method  would  be 
to  ignore  a  measurement  if  there  were  multiple  peaks  within  a  pre-determined  area, 
this  area  would  be  determined  by  the  uncertainty  of  the  system  model. 

4-4  Magnetic  Field  Aided  Trajectory  Estimation 

Two  tests  were  developed  to  demonstrate  the  concepts  from  Chapter  III  and 
the  magnetic  map  information  generated  above.  The  first  test  consisted  of  two  Monte 
Carlo  simulations  that  used  simulated  measurements  generated  from  the  map  data 
to  estimate  the  trajectory  of  the  vehicle.  The  second  test  used  real  magnetic  field 
intensity  measurements  to  estimate  the  trajectory  of  a  vehicle.  Both  tests  used  the 
parameters  shown  in  Table  4.1,  with  the  exception  of  the  vehicle  starting  position. 
The  values  shown  in  Table  4.1  for  the  vehicle  starting  location  are  the  values  used  for 
the  simulated  measurements  case.  For  the  case  with  real  measurements,  there  were 
two  different  starting  points,  (Ad, l'i)  =  (0, 1.524)  and  (A2,l2)=(0, 1.905),  all  in  meters 
and  with  (0,0)  being  the  bottom  left  corner  of  the  hallway  as  oriented  in  Figure  3.2. 
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4-4-1  Generated  Trajectory  Simulation.  There  are  three  basic  steps  used  in 
the  Monte  Carlo  Simulations.  The  first  step  is  the  trajectory  generation.  Secondly, 
the  magnetic  field  intensity  measurements  are  calculated  from  the  map  data  using 
the  locations  supplied  by  the  generated  trajectory  (with  an  appropriate  amount  of 
measurement  noise  added).  The  final  step  in  this  simulation  is  implementing  the 
KF  with  the  system  model  and  magnetic  aided  position  algorithm  developed  in  Sec¬ 
tions  3.3  and  3.4.  This  simulated  approach  allows  for  large  numbers  of  simulations 
to  be  executed,  which  aids  in  determining  the  filter’s  performance.  If  the  simulation 
is  representative  of  the  actual  system,  Monte  Carlo  simulations  show  the  expected 
performance  of  a  system  over  a  wide  range  of  scenarios  and  conditions.  For  this  im¬ 
plementation,  taking  measurements  along  100  known  trajectories  is  unrealistic  with 
respect  to  time  and  instrumentation.  The  location  of  the  magnetic  measurement  must 
be  known  when  it  is  collected.  Based  on  the  measurement  update  interval  of  the  filter, 
each  trajectory  requires  77  different  measurements.  This  would  amount  to  collect¬ 
ing  7700  different  points  of  magnetic  data.  With  the  current  test  equipment,  taking 
measurements  for  one  trajectory  (77  measurements)  takes  about  1  hour.  A  100  run 
Monte  Carlo  simulation  takes  about  2  hours.  At  the  very  least,  the  Monte  Carlo  sim¬ 
ulation  will  verify  that  the  algorithm  operates  as  intended,  and  it  provides  a  baseline 
for  comparison  when  real  data  is  used.  By  understanding  how  the  algorithm  should 
perform  under  the  best  conditions,  when  the  real  data  is  used  differences  between  the 
two  can  be  used  to  determine  areas  of  the  simulation  that  do  not  accurately  depict 
what  is  really  happening  or  to  help  identify  possible  limitations  of  the  algorithm. 
Once  these  limitations  are  understood,  they  can  be  investigated  further  to  create  a 
more  representative  simulation,  which  would  provide  a  more  accurate  algorithm. 

While  a  Monte  Carlo  simulation  is  a  good  tool  for  the  reasons  discussed,  an  un¬ 
derstanding  of  its  limitations  is  key  to  using  this  tool.  The  first  key  is  to  understand 
that  there  is  never  a  true  replacement  for  real  data,  but  simulated  data  can  work,  in 
conjunction  with  real  data,  to  aid  in  getting  a  more  complete  picture,  without  the  time 
needed  to  collect  large  amounts  of  real  data.  If  the  simulation  does  not  adequately 
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match  the  real  system,  the  simulation  results  will  be  invalid  and  not  provide  useful 
information.  In  this  case,  if  the  measurement  noise  is  not  characterized  correctly  the 
simulation  results  would  be  inaccurate  compared  to  the  real  data  tests.  Another  area 
of  the  simulation  that  could  be  inaccurate  comes  from  the  fact  that  if  the  measure¬ 
ment  data  falls  between  grid  points  on  the  map,  the  magnetic  measurement  at  that 
particular  point  is  found  by  interpolating  between  two  points  that  may  have  already 
been  interpolated.  While  interpolation  data  is  valid  to  some  degree,  as  discussed  in 
Section  3.2.2,  interpolating  interpolated  data  introduces  another  source  of  error  into 
the  system  that  is  not  modeled.  With  these  differences  understood,  the  next  step  is  to 
describe  the  setup  and  show  the  results  of  implementing  the  Monte  Carlo  simulation. 

44.1.1  Trajectory  Generation.  A  nominal  trajectory  for  this  simula¬ 
tion  was  defined  as  down  the  center  of  the  main  hallway,  then,  following  a  90°  turn, 
down  the  center  of  the  side  hallway.  The  nominal  trajectory  is  used  to  develop  the 
dynamics  model  of  the  system,  as  described  in  Section  3.3.  Using  the  process  noise, 
as  described  in  Section  3.3,  the  dynamics  model  is  perturbed  by  white  driving  noise  to 
create  a  random  trajectory  that  is  centered  around  the  nominal  trajectory.  The  initial 
attempt  at  generating  this  trajectory  used  aa  =  -08  m/s 2 .  The  initial  attempt  failed 
because  the  vehicle  trajectory  had  no  control  to  keep  it  within  the  confines  of  the 
hallway,  which  is  physically  impossible  with  a  real  vehicle.  To  overcome  this  physical 
reality,  the  trajectory  was  controlled  to  keep  within  the  physical  boundaries  of  the 
hallway.  In  addition  to  the  control,  if  a  trajectory  was  found  to  break  the  physical 
barrier  of  the  hallway,  it  was  rejected  and  the  trajectory  was  regenerated.  Figure  4.11 
shows  a  trajectory  generated  using  this  method. 

44.1.2  Simulated  Magnetic  Field  Intensity  Measurements.  The  mag¬ 
netic  aided  position  algorithm  requires  magnetic  held  intensity  measurements.  The 
measurements  were  generated  from  the  magnetic  held  intensity  data  used  to  develop 
the  magnetic  held  intensity  maps  mentioned  previously.  At  each  point  along  the  gen¬ 
erated  trajectory,  a  magnetic  held  intensity  was  found  by  matching  the  trajectory 
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Figure  4.11:  Example  simulated  trajectory  used  to  test  the  filter’s  ability  to  track 
a  random  trajectory. 

location  with  a  location  on  the  map  of  the  hallway.  If  the  data  fell  in  between  grid 
points,  the  magnetic  field  intensity  was  interpolated  at  that  particular  point.  Once 
the  true  magnetic  field  intensity  (the  true  magnetic  field  for  this  simulation  was  as¬ 
sumed  to  be  the  magnetic  field  intensity  data  collected  for  the  construction  of  the 
magnetic  field  intensity  map)  was  calculated  for  a  given  location,  the  white,  Gaussian 
measurement  noise  was  added  to  each  measurement  to  simulate  the  measurement 
coming  from  a  magnetometer. 

4-4-1 -3  Monte  Carlo  Results  of  Simulated  Trajectories  and  Measurements. 

Monte  Carlo  (MG)  simulations  are  used  to  determine  if  a  system  or  algorithm  will 
operate  as  desired.  For  this  research,  100  MG  runs  were  completed  using  two  differ¬ 
ent  scenarios.  Both  scenarios  used  the  magnetic  aided  position  algorithm  as  described 
throughout  this  research.  However,  the  first  was  executed  without  knowledge  of  the 
trajectory  making  the  turn  onto  the  side  hallway,  while  the  second  used  a  control 
input  to  make  the  turn. 

As  seen  in  Figure  4.12,  the  positioning  solution  generated  by  the  magnetic  aided 
positioning  algorithm  achieves  a  root- mean-square  (RMS)  position  error  of  less  than 
.4  meters,  when  the  filter  is  not  supplied  with  turn  information.  When  the  turn 
command  is  provided  to  the  filter,  the  RMS  values  (shown  in  Figure  4.13)  drop  below 
.2  meters  once  the  initial  positioning  error  is  resolved.  These  two  figures  show  that  the 
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Figure  4.12:  The  RMS  values  for  the  magnetic  aided  position  algorithm  without  a 
turn  command  provided  to  the  filter.  Notice  the  decrease  in  performance  following 
the  turn  at  approx.  80  seconds. 


magnetic  aided  positioning  algorithm  is  able  to  generate  sub-meter  level  positioning 
solutions. 

Figure  4.14  shows  the  ensemble  mean  position  error,  the  ensemble  standard  de¬ 
viation,  and  the  RMS  value  of  the  filter  uncertainties  for  all  100  MC  runs,  when  no 
turn  information  was  provided  to  the  filter.  Figure  4.15  shows  the  ensemble  mean 
position  error,  the  ensemble  standard  deviation,  and  the  RMS  value  of  the  filter  un¬ 
certainties.  For  reference,  the  turn  takes  place  around  80  seconds,  depending  on  the 
randomly  generated  trajectory.  The  mean  error  associated  with  this  filter  was  calcu¬ 
lated  by  subtracting  the  true  trajectory  from  the  estimated  trajectory.  Figure  4.14 
shows  that  the  magnetic  aided  position  algorithm  accurately  estimates  the  vehicle’s 
position  uncertainty  for  the  entire  test  environment.  As  mentioned  in  the  beginning 
of  Section  4.4.1,  the  goal  of  the  MC  simulation  is  to  show  that  the  magnetic  aided 
position  algorithm  does  operate  properly.  Generally,  a  KF  is  said  to  operate  properly 
if  the  filter  uncertainty  accurately  models  the  ensemble  standard  deviation  of  the  re¬ 
sults.  The  filter’s  estimate  of  the  uncertainty  tracks  the  ensemble  standard  deviation 
very  well  along  the  X-direction  for  both  the  filter  that  receives  the  turn  command  and 
the  filter  that  does  not,  Figures  4.14  and  4.15  respectively.  However,  in  both  cases 
the  Y -axis  filter  estimate  is  less  optimistic  in  its  estimate  than  the  actual  ensemble 


51 


Time  (sec) 

Figure  4.13:  The  RMS  values  for  the  magnetic  aided  position  algorithm  with  a  turn 
command  provided  to  the  filter.  Notice  the  smaller  decrease  in  accuracy  following 
the  turn. 

statistics.  While  the  estimation  by  both  filters  seems  to  not  be  impacted  by  this,  both 
achieve  sub-meter  RMS  errors,  understanding  this  minor  discrepancy  is  necessary  to 
understanding  how  well  the  MC  results  relate  to  the  real  system. 

The  main  source  of  the  discrepancy  between  the  ensemble  standard  deviation 
and  the  filter’s  uncertainty  estimate  was  found  to  be  due  to  a  difference  between 
how  the  trajectory  was  generated  and  the  model  used  in  the  KF.  As  mentioned  in 
Section  4.4. 1.1,  the  random  trajectories  were  controlled  to  keep  them  inside  the  con¬ 
fines  of  the  hallway.  The  control  action  is  only  applied  to  the  Y-axis  portion  of  the 
trajectory  along  the  main  hallway  and  the  A^-axis  portion  of  the  trajectory  along 
the  side  hallway.  The  random  trajectory  is  generated  using  the  method  described  in 
Section  4.4. 1.1.  After  each  time  step,  the  distance  between  the  center  of  the  hallway 
and  the  new  position  is  compared.  The  distance  between  the  two  is  then  multiplied 
by  a  scale  factor  and  subtracted  from  the  new  generated  position.  The  result  of  this 
process  is  that  the  filter  is  estimating  its  uncertainty  based  on  an  acceleration  uncer¬ 
tainty  of  .08  meters  per  second  squared,  when  the  actual  trajectory,  for  all  simulated 
runs,  is  varying  less  than  this.  Therefore,  the  MC  runs  have  shown  that  the  magnetic 
aided  position  algorithm  can  provide  positioning  solutions  within  .2  meters  (RMS) 
when  the  filter  has  knowledge  of  the  control  inputs.  In  addition  to  this  positioning 


52 


8 


-0.5 - 1 - 1 - 1 - 1 - 

0  20  40  60  80  100 

Time  (sec) 


Ensemble  lcr  band 
Filter  lcr  band 


-0.5  U - 1 - 1 - 1 - 1 - 1 - 

0  20  40  60  80  100 

Time  (sec) 

Figure  4.14:  Ensemble  Statistics:  The  filter’s  ability  to  track  the  trajectory  is 

contingent  on  the  filter  having  the  correct  system  model.  In  this  case,  the  filter 
model  did  not  include  any  information  about  the  90°  turn. 

accuracy,  the  MC  runs  have  also  shown  that  the  model  used  in  the  simulation  is  cor¬ 
rect,  with  the  exception  of  the  control  on  the  trajectory  generation.  With  the  MC 
results  understood,  the  next  step  is  to  examine  how  the  algorithm  performs  when  real 
data  is  used. 

4-4-2  Real  Trajectory  and  Measurements.  The  second  test  used  the  same 
process  and  algorithms  as  the  first,  but  the  trajectory  was  not  generated  via  simula¬ 
tion.  Instead,  the  second  test  used  a  real  trajectory  that  took  measurements  as  the 
cart  moved  along  the  pre-described  trajectory.  There  were  two  different  trajectories 
used  to  test  the  algorithm  using  real  measurements.  Both  trajectories  were  benign 
in  their  variation  around  the  hallway,  as  the  filter’s  ability  to  estimate  its  position, 
given  good  measurements,  was  shown  via  the  MC  simulations.  The  goal  of  using  real 
measurements  was  to  show  that  the  magnetic  held  intensity  in  the  indoor  environment 
does  vary  enough  by  position,  but  is  stable  enough  in  time  to  support  using  a  map 
collected  at  a  previous  point  in  time.  The  map  data  used  in  this  portion  is  the  same 
map  data  used  previously,  but  the  measurements  were  collected  on  5  December  2008. 

44.2. 1  Trajectory  Generation.  The  first  trajectory  used  to  demon¬ 
strate  the  use  of  real  measurements  is  shown  in  Figure  4.16.  To  ensure  that  the  tra- 
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Figure  4.15:  Ensemble  Statistics:  The  filter’s  ability  to  track  the  trajectory  is 

contingent  on  the  filter  having  the  correct  system  model.  In  this  case,  the  system 
model  is  correct  and  the  filter  and  ensemble  statistics  match  fairly  well,  which  shows 
the  system  model  is  accurate. 

jectory  used  for  truth  matched  the  trajectory  traveled,  the  measurements  needed  to 
be  taken  from  known  locations.  The  simulation  described  previously  in  Section  4.4.1 
used  a  time  step  of  .1  seconds,  an  update  rate  of  1.5  sec,  and  a  forward  vehicle  veloc¬ 
ity  of  .5  meters  per  second.  In  order  to  match  this  criteria  with  a  real  trajectory,  a 
measurement  needed  to  be  taken  at  every  second  grid  point.  This  was  determined  by 
calculating  the  distance  traveled  between  updates  at  the  nominal  velocity,  (1.5  sec¬ 
onds  x  .5  meters  per  second),  the  result  of  this  showed  that  a  measurement  needed  to 
be  taken  every  .750  meters.  Recall  that  the  grid  points  were  located  .381  meters  (15 
in)  apart.  The  distance  between  two  grid  points  was  .762  meters,  which  falls  within 
the  bounds  of  the  system  model. 

Using  this  trajectory,  the  results,  shown  in  Figure  4.17,  show  that  the  real 
measurements  are  good  enough  to  keep  the  vehicle  in  the  hallway,  but  there  are  a 
couple  of  places  where  the  error  exceeds  2  meters.  Figure  4.18  shows  the  measurement 
residuals  of  the  first  trajectory  (the  residuals,  in  this  case,  are  found  by  subtracting 
the  post-update  position  estimate  from  the  position  measurement  found  using  the 
magnetic  aided  position  algorithm).  The  locations  corresponding  to  the  large  position 
errors  are  associated  with  the  locations  that  had  large  measurement  residuals.  This 
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Figure  4.16:  Real  trajectory  near  center  of  hallway.  This  trajectory  was  the  first 
trajectory  used  to  demonstrate  the  performance  of  the  magnetic  aided  position  algo¬ 
rithm  using  real  measurements. 

means  the  magnetic  field  intensity  measurement  correlated  with  a  position  different 
from  the  filter’s  estimated  position  by  a  large  margin.  Figures  4.4  and  4.6  show  that 
there  is  not  much  variation  in  the  middle  of  the  hallway.  This  lack  of  variation  could 
lead  to  the  large  residuals  seen  in  Figure  4.18. 

To  help  determine  if  the  lack  of  variation  was  really  the  problem,  the  second 
trajectory,  pictured  in  Figure  4.19,  goes  down  the  right-hand  side  of  the  hallway. 
The  same  techniques  used  to  generate  the  trajectory  pictured  in  Figure  4.16  were 
used  to  generate  this  trajectory.  The  position  errors  associated  with  going  down  the 
side  of  the  hallway  is  shown  in  Figure  4.20.  The  result  is  smaller  errors  and  smaller 
residuals,  as  shown  in  Figure  4.21.  However,  the  errors  are  still  significantly  larger 
than  the  errors  generated  using  the  MC  simulation. 

The  trajectory  estimation  using  real  measurements  was  conducted  using  the 
same  system  model  as  that  of  the  MC  simulation.  The  vehicle  was  defined  as  moving 
at  .5±.08  meters  per  second.  From  Figures  4.16  and  4.19,  it  can  be  seen  that  there  was 
virtually  no  variation  in  the  cross-track  direction.  This  means  in  the  main  hallway, 
there  was  very  little  variation  in  the  Y -direction  and  on  the  side  hallway  there  was 
very  little  variation  in  the  X-direction.  Using  this  observation,  the  system  model  was 
changed  to  reflect  this  (cra  was  changed  from  .08  meters  per  second  squared  to  .02 
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Figure  4.17:  Position  error  plots  showing  the  filter  performance  using  non- simulated 
measurements  from  the  trajectory  shown  in  Figure  4.16. 

meters  per  second  per  squared).  Figures  4.22  and  4.23  show  the  improved  results  for 
the  trajectory  down  the  middle  of  the  hallway  and  the  trajectory  down  the  side  of  the 
hallway,  respectively.  The  position  error  for  the  first  real  trajectory  went  from  having 
a  maximum  position  error  of  over  two  meters  to  having  a  position  error  of  a  meter  or 
less.  The  second  trajectories  performance  improved  just  as  dramatically.  Originally 
it  had  a  maximum  error  of  approximately  1.5  meters,  and  now  the  maximum  error 
for  that  trajectory  is  .6  meters.  This  illustrates  the  importance  of  having  the  correct 
system  model  in  the  KF. 

4-5  Leader- Follower  Algorithm  Implementation 

The  leader-follower  algorithm  is  a  relative  positioning  algorithm  that  uses  mag¬ 
netic  field  data,  measured  by  the  leader,  to  aid  the  position  estimate  of  a  follower. 
The  follower  then  uses  this  position  estimate  to  control  its  path  to  remain  close  to  the 
path  of  the  lead  vehicle.  The  implementation  as  designed  in  Section  3.5  would  not 
work  for  a  cra  of  .08  meters  per  second  squared.  The  reason  for  this  is  that  the  second 
vehicle  must  follow  the  trajectory  of  the  first  vehicle  reasonably  well,  or  the  second 
vehicle  will  not  remain  inside  the  span  of  the  leader’s  measurements.  If  the  follower 
is  outside  of  the  span  of  the  leader’s  measurements,  the  follower  will  not  be  able  to 
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Figure  4.18:  Measurement  residuals  of  the  first  non-simulated  trajectory.  The  large 
position  errors  in  Figure  4.17  correspond  to  a  bad  measurement  value.  These  can  be 
attributed  to  areas  with  little  magnetic  variation. 

estimate  its  position.  After  simulating  the  leader- follower  algorithm  using  different 
values  for  aa,  the  largest  value  successfully  implemented  was  .02  meters  per  second 
squared,  which  was  used  in  both  the  lead  vehicle’s  system  model  and  the  follower 
vehicle’s  system  model. 

4-5.1  Leader- Follower  Performance  Analysis.  With  the  lead  vehicle  tra¬ 
jectory  constant  and  magnetic  measurements  taken  along  this  trajectory  with  three 
sensors,  the  follower  tracking  ability  was  tested  using  50  simulation  runs  and  a  ua  of 
.02  meters  per  second  squared.  Out  of  the  50  runs,  zero  were  completed  or  successful. 
A  completed  run  is  defined  as  a  run  where  the  follower  makes  it  at  least  16  meters 
down  the  side  hallway.  A  successful  run,  for  this  implementation,  is  defined  as  a  run 
that  tracks  the  leader  at  least  10  meters  down  the  side  hallway,  which  is  55  percent  of 
the  side  hallway  length  and  approximately  90  percent  of  the  total  path.  The  reason 
for  the  10  meter  threshold  is  there  is  a  section  of  the  side  hallway  (right  around  10 
meters)  that  does  not  have  a  lot  of  variation.  As  mentioned  in  Section  4.2.1,  if  an 
area  does  not  have  large  variations,  the  magnetic  aided  position  algorithm  will  not 
accurately  determine  its  position.  For  the  current  leader-follower  implementation, 
this  problem  is  magnified,  because  the  follower  vehicle  is  trying  to  ascertain  where  it 
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Figure  4.19:  Real  trajectory  near  right  side  of  hallway.  This  trajectory  was  the 

second  trajectory  used  to  demonstrate  the  performance  of  the  magnetic  aided  position 
algorithm  using  real  measurements.  This  trajectory  takes  advantage  of  the  large 
variations  present  near  structures  such  as  walls  and  floors. 

is  with  respect  to  the  leader’s  estimated  trajectory.  If  the  follower  picks  the  wrong 
location,  the  control  input  could  command  the  follower  in  the  wrong  direction. 

For  example,  if  the  position  estimate  generated  by  the  follower  vehicle  is  to  the 
right  of  the  leader’s  estimated  trajectory,  but  the  follower  vehicle  is  actually  to  the  left 
of  the  leader’s  estimated  trajectory,  then  the  control  input  generated  by  the  magnetic 
aided  position  algorithm  will  command  the  follower  vehicle  to  go  further  to  the  left, 
which  will  take  the  follower  vehicle  further  from  the  reference  trajectory.  If  there  is 
enough  variation  in  the  magnetic  field  at  this  commanded  position,  the  magnetic  aided 
position  algorithm  will  realize  the  follower  is  to  the  left  of  the  reference  trajectory  with 
the  next  estimate  and  correct  its  position  accordingly.  If  there  is  not  enough  variation, 
then  the  magnetic  aided  position  algorithm  could  estimate  the  vehicle’s  position  as 
being  to  the  right  of  the  reference  trajectory  again  and  command  the  vehicle  further 
to  the  right.  This  phenomenon  was  the  cause  of  all  unfinished  runs  in  the  lead-follower 
implementation.  Figure  4.24  shows  one  case  of  this  phenomenon. 

To  avoid  the  phenomenon  created  by  the  lack  of  large  variations,  two  different 
approaches  were  implemented.  The  first  implementation  was  a  fix  to  the  algorithm 
that  moves  the  actual  vehicle  back  to  the  center  of  the  hallway.  This  method,  as 
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Figure  4.20:  Position  error  plots  showing  the  filter  performance  using  the  non- 

simulated  measurements  from  the  trajectory  shown  in  Figure  4.19. 

implemented,  is  primarily  a  tool  to  show  that  the  algorithm  would  function  properly 
if  the  vehicle  managed  to  move  back  to  the  path  of  the  lead  vehicle  (perhaps  by  turning 
away  from  walls).  In  the  simulation,  this  was  done  by  monitoring  the  vehicle’s  actual 
position  in  the  hallway.  If  the  vehicle  reached  a  point  where  it  would  be  physically 
outside  the  confines  of  the  hallway,  much  like  the  case  of  the  generated  trajectory 
in  Section  4.4.1,  then  the  vehicle’s  position  would  be  reset  to  the  opposite  side  of 
the  center  of  the  hallway.  This  implementation  improved  the  performance,  but  did 
not  eliminate  the  failure  mode.  The  number  of  successful  runs  out  of  50  using  this 
approach  went  from  0  to  18  and  the  number  completed  went  from  0  to  2. 

The  second  approach  used  an  additional  sensor  in  the  lead  vehicle.  The  goal 
of  this  approach  was  to  show  that  the  addition  of  a  fourth  sensor  would  widen  the 
sensed  magnetic  held  by  the  leader,  allowing  for  the  follower  to  be  farther  from  the 
leader’s  trajectory,  but  still  have  valid  magnetic  held  measurements.  This  method 
improved  the  performance  over  the  three  sensor  case,  with  and  without  the  position 
reset.  Out  of  50  runs  using  this  approach,  9  were  completed  and  18  were  successful 
in  tracking  the  leader  for  over  90  percent  of  the  total  trajectory. 

To  increase  the  successful  tracking  percentage,  the  second  approach  was  com¬ 
bined  with  the  hrst  approach.  By  adding  a  fourth  sensor  and  resetting  the  vehicle’s 
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Figure  4.21:  Measurement  residuals  of  the  second  non-simulated  trajectory.  The 

large  position  errors  in  Figure  4.20  are  not  as  large  as  those  in  Figure  4.17.  This  is 
due  to  greater  magnetic  held  variation  along  the  side  of  the  hallway,  which  equate  to 
more  accurate  position  measurements. 

position  if  the  vehicle  “hit”  a  wall,  the  number  of  successful  runs  was  36  and  the 
number  of  completed  runs  was  23.  Table  4.2  shows  the  different  levels  of  performance 
achieved  by  each  implementation.  As  expected,  adding  more  sensors  and  having 
some  way  of  monitoring  the  validity  of  a  position  estimate  (i.e.,  residual  monitoring, 
likelihood  thresholding,  etc.)  has  the  highest  level  of  success. 

4-5.2  Leader- Follower  Estimation  Error.  As  mentioned  in  previous  sections, 
the  goal  of  the  leader- follower  algorithm  is  to  have  the  follower  track  the  trajectory  of 
the  leader.  However,  since  the  lead  vehicle  only  has  an  estimate  of  its  true  trajectory 
(this  is  the  reference  trajectory  passed  to  the  follower),  the  follower  can  only  track  the 
estimated  trajectory  of  the  leader.  Therefore,  the  position  estimate  generated  by  the 
follower  is  in  the  wrong  reference  frame  to  compare  it  to  the  leader’s  true  trajectory. 
The  goal  of  the  leader- follower  algorithm  is  to  show  how  well  the  follower  follows  the 
path  of  the  leader.  In  order  to  do  this,  the  leader’s  estimated  trajectory  must  be 
resolved  in  the  reference  frame  of  leader’s  actual  trajectory.  Once  this  transformation 
is  executed,  the  follower’s  position  can  be  determined  with  respect  to  the  leader’s 
position.  The  difference  in  reference  frame  means  that  each  position  measurement 
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Table  4.1:  Parameter  values  used  for  the  simulations  that  demonstrate  the  mag¬ 

netically  aided  INS. 


Parameter 

Value 

Time  Step  At 

0.1  sec 

Initial  A"-Direction  Velocity  (Vx) 

.5  m/s 

Initial  V-Direction  Velocity  (Vy) 

0  m/s 

Initial  Filter  X-Position  (Px) 

0  m 

Initial  Filter  V-Position  (Py) 

0  m 

Initial  Vehicle  X-Position  (Px) 

0  m 

Initial  Vehicle  V-Position  (Py) 

1.145  m 

Initial  X-Position  Uncertainty  (&px) 

1.5  m 

Initial  V-Position  Uncertainty  (crpY) 

1.5  m 

Acceleration  Uncertainty  Both  Axes  (aa) 

.08  m/s2 

Magnetometer  Measurement  Noise  (<Jmeas) 

1  milli  —  gauss 

Filter  Update  Interval 

1.5  sec 

Table  4.2:  Performance  comparison  of  the  leader- follower  algorithm,  using  different 
techniques  to  aid  the  magnetic  aided  position  algorithm  in  areas  of  small  variations. 
The  number  of  completed  runs  are  the  runs  where  the  follower  made  it  at  least  16 
meters  down  the  side  hallway,  whereas  a  successful  run  was  considered  runs  that  made 
it  to  10  meters,  the  successful  run  column  includes  the  completed  runs. 


Implementation 

Number  of 

Complete 

Runs 

Number 

of 

Successful 

Runs 

Number 

Past 

Turn 

Number 

Not  Making 

Turn 

Three  Sensors  No  Pos.  Reset 

0  (0%) 

0  (0%) 

0  (0%) 

50  (100%) 

Three  Sensors  Pos.  Reset 

2  (4%) 

18  (36%) 

33  (66%) 

17  (34%) 

Four  Sensors  No  Pos.  Reset 

9  (18%) 

18  (36%) 

38  (76%) 

12  (24%) 

Four  Sensors  Pos.  Reset 

23  (46%) 

36  (72%) 

41  (82%) 

9  (18%) 

61 


Figure  4.22:  Position  error  plots  showing  the  filter  performance  using  the  non- 

simulated  measurements  from  the  trajectory  shown  in  Figure  4.16  and  <ja  =  .02  m/s2. 

generated  by  the  magnetic  aided  position  algorithm  is  offset  by  the  difference  between 
the  leader’s  true  trajectory  and  the  leader’s  estimated  trajectory.  The  error  between 
the  follower’s  position  estimate  and  the  leader’s  position  estimate  is  calculated  for  use 
in  the  PID  controller  as  described  in  Section  3.5.2.  ffowever,  to  view  the  follower’s 
trajectory  with  respect  to  the  leader’s  true  trajectory  the  follower’s  actual  position 
must  be  transformed  into  the  leader’s  reference  frame.  The  process  used  to  complete 
this  transformation  can  be  viewed  in  Appendix  A. 

The  follower’s  true  trajectory  is  plotted  with  the  leader’s  estimated  trajectory 
in  Figure  4.25a.  However,  as  mentioned  previously,  the  result  in  Figure  4.25a  does 
not  show  how  well  the  follower  tracked  the  leader.  Figure  4.25b  shows  the  corrected 
follower  trajectory.  The  case  shown  is  for  a  leader  trajectory  that  veers  to  the  right  of 
the  reference  trajectory  along  the  main  hallway  and  then  veers  to  the  left  on  the  side 
hallway.  There  is  a  gap  in  the  follower’s  trajectory  as  the  actual  trajectory  crosses 
over  the  reference  trajectory  in  Figure  4.25.  This  is  a  result  of  the  calculation  used 
to  transform  the  follower’s  trajectory  into  the  leader’s  reference  frame.  If  the  two 
vehicle’s  were  actually  observed  through  this  process,  there  would  be  no  break  in 
the  data.  The  data  presented  here  is  from  the  four  sensor  implementation,  with  and 
without  the  position  reset. 
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Figure  4.23:  Position  error  plots  showing  the  filter  performance  using  the  non- 

simulated  measurements  from  the  trajectory  shown  in  Figure  4.19  and  <ja  =  .02  m/s2. 

To  further  illustrate  the  leader-follower  implementation,  Figure  4.26  shows  the 
same  information  as  Figure  4.25,  but  for  a  true  trajectory  that  veers  to  the  left  along 
the  main  hallway.  Notice  the  sawtooth-like  variation  in  the  follower’s  trajectory.  This 
is  introduced  by  the  position  reset  routine  described  in  Section  4.5.1.  The  two  figures 
just  presented  demonstrate  that  the  transformation  works  for  almost  all  situations. 
The  only  error  in  this  process  comes  from  a  singularity  when  the  two  leader’s  trajec¬ 
tories  cross  one  another. 

Now  that  the  trajectories  are  in  the  correct  frame  of  reference,  the  actual  errors 
between  the  leader’s  true  trajectory  and  the  follower’s  true  trajectory  can  be  calcu¬ 
lated.  The  errors  presented  in  Figures  4.27  and  4.28  were  calculated  by  subtracting  the 
leader’s  true  value  from  the  follower’s  true  value.  Recall  that  the  goal  of  the  control 
action  was  to  minimize  the  cross-track  error  between  the  follower’s  actual  trajectory 
and  the  leader’s  actual  trajectory.  Using  the  transformation  shown  in  Appendix  A, 
the  cross-track  errors  are  shown  to  be  less  than  .4  meters.  These  results  are  typical 
of  the  cases  that  completed  a  successful  run. 
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Figure  4.24:  Example  of  the  failure  mode  induced  by  small  magnetic  variations. 

Notice  the  position  measurement  and  estimate  are  on  the  opposite  side  of  the  reference 
trajectory.  The  tracking  command,  U2,  commands  the  vehicle  to  turn  away  from  the 
reference  trajectory. 


4  •  6  Summary 

This  chapter  demonstrated  that  the  magnetic  aided  position  algorithm  can  pro¬ 
vide  position  solutions  with  sub-meter  level  accuracy.  Using  a  map  of  an  entire  area 
to  estimate  a  random  trajectory  produced  RMS  errors  of  less  than  .3  meters,  while  the 
leader- follower  algorithm  produced  errors  less  than  .4  meters,  as  long  as  the  follower 
stayed  within  the  range  of  measurements  provided  by  the  leader. 
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(a)  Uncorrected  Reference  Frame 


(b)  Corrected  Reference  Frame 


Figure  4.25:  The  follower’s  trajectory  shown  in  both  reference  frames,  (a)  Shows 
the  follower’s  trajectory  in  the  leader’s  estimated  trajectory  reference  frame  and  (b) 
shows  the  follower’s  reference  frame  resolved  in  the  leader’s  true  trajectory  frame. 
The  leader’s  true  trajectory,  in  this  case,  veered  to  the  right  along  the  main  hallway 
and  to  the  left  on  the  side  hallway. 


(a)  Uncorrected  Reference  Frame 


(b)  Corrected  Reference  Frame 


Figure  4.26:  The  follower’s  trajectory  shown  in  both  reference  frames,  (a)  Shows 
the  follower’s  trajectory  in  the  leader’s  estimated  trajectory  reference  frame  and  (b) 
shows  the  follower’s  reference  frame  resolved  in  the  leader’s  true  trajectory  frame. 
The  leader’s  true  trajectory,  in  this  case,  veered  to  the  left  along  the  main  hallway 
and  to  the  left  on  the  side  hallway.  The  sawtooth  variations  along  the  side  hallway 
are  due  to  the  position  reset  algorithm. 
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Figure  4.27:  The  position  errors  between  the  follower  trajectory  and  the  leader 

trajectory.  The  rise  in  error  following  the  turn  at  80  seconds  is  caused  by  no  control 
along  the  path  of  travel.  This  case  used  the  position  reset  algorithm. 


Figure  4.28:  The  position  errors  between  the  follower  trajectory  and  the  leader 

trajectory.  The  rise  in  error  following  the  turn  at  80  seconds  is  caused  by  no  control 
along  the  path  of  travel.  This  case  did  not  use  the  position  reset  algorithm. 
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V.  Conclusions  and  Recommendations 


This  research  has  demonstrated  the  feasibility  of  using  magnetic  field  variations 
to  aid  inertial  navigation  systems.  The  method  demonstrated  modified  a  multi¬ 
beam  terrain  navigation  approach  to  take  advantage  of  the  three  distinct  measure¬ 
ments  provided  by  the  three-axis  magnetometers.  The  adapted  approach  uses  a  KF 
and  a  magnetic  aided  position  algorithm  to  aid  the  inertial  system.  The  magnetic 
aided  position  algorithm  relates  the  magnetic  field  intensity  measurements  to  a  spe¬ 
cific  position  through  the  use  of  a  map  of  magnetic  field  intensities  for  each  of  the 
three  magnetometer  axes.  The  maps  were  generated  using  magnetic  field  intensity 
data  collected  at  equally  spaced  grid  points  and  then  interpolated  to  provide  closer 
grid  spacing. 

5.1  The  Magnetic  Aided  Position  Algorithm 

Following  the  generation  of  the  magnetic  held  intensity  maps,  the  position  mea¬ 
surement  update  was  generated  using  the  magnetic  aided  position  algorithm.  The 
magnetic  aided  position  algorithm  calculates  the  position  measurement  by  applying 
Bayes’  rule  to  the  results  of  a  maximum  likelihood  function  and  the  propagated  pdf 
generated  via  the  KF.  The  measurement  update  was  then  incorporated  with  the  KF 
position  estimate  with  the  standard  KF  update  equations.  The  positon  aiding  al¬ 
gorithm,  as  developed,  was  applied  in  three  different  cases:  a  MC  simulation  with 
simulated  measurements  generated  from  the  magnetic  held  map,  a  test  using  real 
measurements,  and  a  leader-follower  simulation. 

5.2  Magnetic  Aided  Position  Algorithm  Tests 

The  MC  simulation  showed  that  the  inertial  system,  with  aiding  provided  by 
the  magnetic  aided  position  algorithm,  was  able  to  successfully  track  100  random 
trajectories  with  RMS  errors  of  less  than  .3  meters. 

The  test  of  the  magnetic  aided  position  algorithm  using  real  measurements 
helped  to  verify  that  the  process  used  for  the  MC  simulation  was  valid,  as  well  as 


67 


to  illustrate  that  indoor  magnetic  field  intensities  are  stable  enough  to  be  mapped, 
stored,  and  used  at  a  later  date  for  position  sub-meter  position  solutions. 

The  leader- follower  algorithm  demonstrated  one  application  of  this  magnetic 
aided  position  algorithm.  The  leader  collects  magnetic  field  intensity  data  as  it  moves 
through  an  area  and  then  passes  this  information  to  a  second  vehicle.  The  follower 
then  uses  the  magnetic  data,  the  estimated  trajectory,  and  any  turn  commands  from 
the  leader  to  track  the  lead  vehicle  through  an  environment.  The  leader- follower 
implementation  requires  both  the  leader  and  follower  to  have  more  accurate  speed 
control  (<ra  =  .02  m/s2)  than  when  the  position  aided  algorithm  is  implemented 
using  the  map  of  an  entire  area  ( aa  =  .08  m/s2).  The  need  for  less  variation  on 
the  acceleration  of  the  leader  and  follower  is  because  the  map  generated  with  this 
approach  is  a  subset  of  the  entire  area.  If  the  follower  gets  outside  the  bands  of  the 
magnetic  map  generated  by  the  leader,  there  is  no  information  available  to  aid  the 
follower. 

Initially,  the  leader- follower  algorithm  used  three  sensors  on  the  leader  to  mea¬ 
sure  the  magnetic  field.  Upon  implementation,  three  sensors  were  found  to  be  inad¬ 
equate  to  meet  performance  goals.  The  three  sensor  approach  makes  the  magnetic 
aided  position  algorithm  more  susceptible  to,  in  terrain  navigation  terminology,  “flat- 
bottomed”  areas,  which  are  areas  where  the  magnetic  variation  between  points  is  less 
than  the  measurement  noise  of  the  magnetometers.  To  overcome  this  in  simulation, 
a  position  reset  algorithm  was  implemented.  In  addition  to  the  problem  of  “flat- 
bottomed”  areas,  the  three  sensors  did  not  provide  enough  map  coverage  to  achieve 
the  desired  performance  goals.  Therefore,  an  additional  sensor  was  added  to  the 
leader.  By  adding  the  fourth  sensor  and  the  position  reset  algorithm,  performance 
was  increased  dramatically.  Using  this  method  resulted  in  the  follower  tracking  the 
leader  within  .4  meters. 
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5.3  The  Way  Ahead 

While  this  research  has  shown  the  potential  of  using  magnetic  field  intensity 
information  to  aid  indoor  position  information,  there  are  still  several  aspects  that 
should  be  considered.  Among  these  are  the  length  of  time  magnetic  intensity  values 
are  valid,  the  benefits  of  using  heading  reference  information,  ways  to  improve  per¬ 
formance  of  the  leader-follower  algorithm,  and  how  to  overcome  the  “flat-bottomed” 
areas. 

The  length  of  time  between  the  collection  of  the  magnetic  map  data  and  the 
real  measurements  was  two  weeks.  Throughout  the  literature,  there  has  not  been 
much  characterization  of  magnetic  field  variations  in  an  indoor  environment,  except 
to  say  that  the  variations  prevent  electronic  compasses  from  working  properly  [13]. 
While  this  research  shows  the  variations  to  be  stable  enough  for  success  over  a  two 
week  time  period,  it  is  necessary  to  know  when  the  measurements  no  longer  become 
valid.  A  possible  solution  to  the  time  stability  question  would  be  to  subtract  out  the 
Earth’s  changing  field  and  compare  just  the  magnetic  anomalies  present  in  the  indoor 
environment.  This  is  the  method  used  in  [19]  for  the  outdoor  case. 

For  simplification  purposes,  vehicle  heading  information  was  not  directly  used 
in  this  research.  Magnetic  field  intensity  is  a  three  dimensional  vector  at  any  given 
point.  This  vector  is  described  by  the  three- axis  measurements  provided  by  the  mag¬ 
netometers.  If  the  magnetic  intensity  is  collected  at  a  known  location  and  orientation, 
the  heading  of  a  vehicle  can  be  determined  by  finding  the  point  of  maximum  likeli¬ 
hood  using  the  total  magnitude  of  the  magnetic  field  intensity  and  then  finding  the 
orientation  that  matches  the  three- axis  measurement.  The  heading  information  would 
allow  for  a  more  accurate  control  of  the  vehicle  as  it  moves  through  an  environment 
and  provide  more  precise  positioning  information.  However,  from  a  computational 
standpoint,  it  would  require  searching  over  one  more  dimension  (x,y  position  plus 
heading,  as  opposed  to  x,y  position). 
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A  couple  of  methods  already  investigated  for  improving  the  performance  of  the 
leader- follower  implementation  were  adding  an  additional  sensor  to  the  leader  and 
implementing  a  position  reset  algorithm  in  the  second  vehicle.  In  the  simulation, 
the  position  was  reset  by  detecting  when  the  vehicle  hit  the  “wall”  of  the  hallway. 
This  can  be  implemented  on  a  real  vehicle  by  using  a  touch  sensor  on  each  side  of 
the  vehicle.  If  the  right  touch  sensor  is  activated,  the  vehicle  could  be  controlled  to 
the  left  a  prescribed  distance  and  then  the  magnetic  aided  position  algorithm  could 
take  over  control.  This  method  treats  the  symptom  of  the  problem  as  opposed  to  the 
problem  itself,  which  is  a  lack  of  variation  in  the  magnetic  field. 

Adding  an  additional  sensor  on  the  leader  helped  reduce  the  problems  caused 
by  a  lack  of  variation,  but  did  not  eliminate  them.  Another  approach  would  be  to 
add  an  additional  sensor  to  the  follower.  The  second  sensor  would  allow  at  least  two 
different  methods  to  be  used  to  remove  the  ambiguity  seen  in  the  “flat-bottomed” 
areas.  The  first  method  would  be  to  combine  the  measurements  from  the  second 
sensor  with  the  measurements  from  the  first  sensor  in  the  likelihood  function  to  help 
remove  the  ambiguity.  This  approach  comes  from  the  observation  in  [11]  that  more 
measurements  help  to  remove  erroneous  measurements.  The  second  method  would 
be  to  find  the  position  of  each  magnetometer  using  the  likelihood  function  and  then 
combine  that  information  with  the  geometry  of  the  sensor  array.  If  the  positions 
found  using  the  likelihood  function  are  not  feasible  due  to  the  physical  layout  of  the 
system,  then  these  position  updates  could  be  ignored,  or  the  results  of  the  likelihood 
function  could  be  compared  to  find  the  points  that  do  match  the  physical  layout  of 
the  sensor  array. 

Overall,  the  processes  developed  and  implemented  for  this  research  show  that 
magnetic  field  intensity  can  be  used  as  a  viable  source  of  position  data  indoors.  While 
areas  of  limited  magnetic  variation  pose  the  largest  problem  for  this  navigation  ap¬ 
proach,  the  problems  could  be  overcome  by  using  some  of  the  above  approaches  or  by 
combining  this  approach  with  other  indoor  navigation  techniques,  such  as  vision-aided 
navigation. 
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Appendix  A.  Leader-Follower  Coordinate  Transformation 

Figure  A.l  is  a  graphical  representation  of  the  error  transformation,  with  xri/2 
representing  the  position  estimate  of  the  leader,  xactuau/2  representing  the  ac¬ 
tual  trajectory  of  the  lead  vehicle,  and  xpiant  representing  the  follower’s  position  at  a 
given  instant  in  time.  The  subscript  1/2  represent  the  two  closest  positions  to  xpiant 
from  the  respective  trajectories.  With  Figure  A.l  as  a  reference,  the  point  C  is  found 
using  the  procedure  outlined  in  Section  3.5.2,  with  the  position  estimate,  x(f^),  re¬ 
placed  by  xpiant.  The  next  step  is  to  determine  the  distance  between  C  and  B' .  Since 
Xactuai  and  xr  are  discrete,  B'  may  be  between  discrete  points  and  cannot  be  found 
directly.  Therefore,  the  Law  of  Sines  can  be  used  to  find  the  length  B. 

In  order  to  use  the  Law  of  Sines,  which  states 


A  _  B  _  C 
sin(A)  sin(.B)  sin(C')  ’ 


(A.l) 


at  least  two  sides  and  an  angle  or  two  angles  and  one  side  must  be  known.  In  this 
case,  the  length  of  A,  the  length  of  D,  and  the  angle  between  A  and  B  can  be  found 
from  the  known  information.  The  length  of  A  is  calculated  by  finding  the  norm  of 
A,  which  is  the  vector  that  points  from  point  C  to  point  xactUaii-  If  there  exists  two 
vectors,  the  angle  between  them  can  be  calculated.  The  vector  between  C  and  B'  is 
not  known,  but  the  unit  vector  between  C  and  B'  is  known,  it  is  equal  to  the  unit 
vector  of  A p,  uap-  Therefore,  the  angle  between  A  and  «ap,  is  found  using 


6  =  cos 


A  ■  Uap  \ 

Piiwy 


(A.2) 


With  5  and  A  known,  the  next  step  is  to  find  the  angle  opposite  side  A,  a.  The  angle 
a  is  found  by  calculating  the  angle  between  uap  and  up,  which  is  the  unit  vector 
of  the  vector  that  extends  from  xactuai2  to  xactuan,  and  subtracting  that  angle  from 
180°.  Using  Equation  A.l  and  the  values  calculated  for  A,  a,  and  <5,  the  length  of  D 
is  calculated.  The  length  of  D  is  added  to  xactUaii  to  determine  the  location  of  B' . 
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Figure  A.l:  The  geometric  representation  used  to  determine  the  calculations  needed 
to  convert  the  follower’s  trajectory  into  the  leader’s  true  trajectory  frame  of  reference. 

The  vector  B  is  then  calculated  by  subtracting  B'  from  C.  With  B,  the  error  vector 
is  calculated  by  multiplying  the  norm  of  B  times  u&p. 

If  xPiant  is  located  between  the  leader’s  true  trajectory  and  the  leader’s  estimated 
trajectory,  uap  will  point  in  the  direction  opposite  of  the  actual  error.  This  creates  a 
situation  where  5  is  calculated  as  5' .  Therefore,  if  the  value  calculated  for  5  is  greater 
than  90°,  S  is  subtracted  from  180°  and  the  sign  of  u^p  is  reversed. 
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